1 package org.opentrafficsim.road.gtu.lane.tactical.util;
2
3 import java.util.ArrayList;
4 import java.util.List;
5 import java.util.Set;
6
7 import org.djunits.value.vdouble.scalar.Angle;
8 import org.djunits.value.vdouble.scalar.Duration;
9 import org.djunits.value.vdouble.scalar.Length;
10 import org.djunits.value.vdouble.scalar.Speed;
11 import org.djutils.exceptions.Throw;
12 import org.opentrafficsim.base.parameters.ParameterException;
13 import org.opentrafficsim.base.parameters.ParameterTypeDouble;
14 import org.opentrafficsim.base.parameters.ParameterTypeDuration;
15 import org.opentrafficsim.base.parameters.Parameters;
16 import org.opentrafficsim.base.parameters.constraint.NumericConstraint;
17 import org.opentrafficsim.core.geometry.OTSGeometryException;
18 import org.opentrafficsim.core.geometry.OTSLine3D;
19 import org.opentrafficsim.core.geometry.OTSPoint3D;
20 import org.opentrafficsim.core.gtu.GTU;
21 import org.opentrafficsim.core.gtu.RelativePosition;
22 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
23 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
24 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
25 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
26
27 import nl.tudelft.simulation.language.d3.DirectedPoint;
28
29 /**
30 * Utility for tactical planners to implement more precise (in terms of physics) vehicle control.
31 * <p>
32 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
33 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
34 * <p>
35 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 8 jan. 2019 <br>
36 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
37 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
38 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
39 */
40 public final class Steering
41 {
42
43 /** Time step parameter. */
44 static final ParameterTypeDuration DT =
45 new ParameterTypeDuration("DTS", "Steering update time step", Duration.createSI(0.01), NumericConstraint.POSITIVE);
46
47 /** Front tire cornering stiffness. */
48 static final ParameterTypeDouble C_FRONT = new ParameterTypeDouble("C_FRONT", "Front tire cornering stiffness", 80000);
49
50 /** Rear tire cornering stiffness. */
51 static final ParameterTypeDouble C_REAR = new ParameterTypeDouble("C_REAR", "Rear tire cornering stiffness", 80000);
52
53 /**
54 *
55 */
56 private Steering()
57 {
58 // empty constructor
59 }
60
61 /**
62 * Translates a reference trajectory in to steering angles and a resulting trajectory.
63 * @param gtu LaneBasedGTU; GTU
64 * @param params Parameters; parameters
65 * @param steeringState SteeringState; steering state between operational plans
66 * @param referencePlan OperationalPlan; operational reference plan
67 * @param feedbackTable FeedbackTable; table of feedback values
68 * @return actual operational plan
69 * @throws ParameterException undefined parameter
70 */
71 public static OperationalPlan fromReferencePlan(final LaneBasedGTU gtu, final Parameters params,
72 final SteeringState steeringState, final OperationalPlan referencePlan, final FeedbackTable feedbackTable)
73 throws ParameterException
74 {
75 Duration step = Duration.ZERO;
76 DirectedPoint pos;
77 try
78 {
79 pos = referencePlan.getLocation(step);
80 }
81 catch (OperationalPlanException exception)
82 {
83 throw new RuntimeException(exception); // could not happen, we loop inside the plan duration
84 }
85 List<OTSPoint3D> points = new ArrayList<>();
86 points.add(new OTSPoint3D(pos));
87 Angle steeringAngle = steeringState.getSteeringAngle();
88 Angle angularError = steeringState.getAngularError();
89 double angularErrorDerivative = steeringState.getAngularErrorDerivative();
90 Length positionError = steeringState.getPositionError();
91 Speed positionErrorDerivative = steeringState.getPositionErrorDerivative();
92 while (step.lt(referencePlan.getTotalDuration()))
93 {
94 Speed speed;
95 gtu.getLength();
96 gtu.getWidth();
97 gtu.getVehicleModel().getMomentOfInertiaAboutZ();
98 gtu.getRelativePositions().get(RelativePosition.CENTER_GRAVITY).getDx();
99 gtu.getFront();
100 gtu.getRear();
101 try
102 {
103 speed = referencePlan.getSpeed(step);
104 }
105 catch (OperationalPlanException exception)
106 {
107 throw new RuntimeException(exception); // could not happen, we loop inside the plan duration
108 }
109 // TODO: apply math
110
111 points.add(new OTSPoint3D(pos)); // with pos being updated to the end position of the current step
112 step = step.plus(params.getParameter(DT));
113 }
114 steeringState.setSteeringAngle(steeringAngle);
115 steeringState.setAngularError(angularError);
116 steeringState.setAngularErrorDerivative(angularErrorDerivative);
117 steeringState.setPositionError(positionError);
118 steeringState.setPositionErrorDerivative(positionErrorDerivative);
119 OTSLine3D path;
120 try
121 {
122 path = new OTSLine3D(points.toArray(new OTSPoint3D[points.size()]));
123 }
124 catch (OTSGeometryException exception)
125 {
126 throw new RuntimeException("The path has too few or too close points.", exception);
127 }
128 OperationalPlan realPlan;
129 try
130 {
131 realPlan = new OperationalPlan(gtu, path, referencePlan.getStartTime(), referencePlan.getStartSpeed(),
132 referencePlan.getOperationalPlanSegmentList());
133 }
134 catch (OperationalPlanException exception)
135 {
136 // TODO: how to handle a shorter (i.e. speed profile assumes more length) or longer path (i.e. impossible to get
137 // position on reference trajectory)
138 throw new RuntimeException(exception);
139 }
140 // TODO: return new plan
141 return referencePlan;
142 }
143
144 /**
145 * Translates reference points in to steering angles and a resulting trajectory.
146 * @param gtu GTU; GTU
147 * @param params Parameters; parameters
148 * @param steeringState SteeringState; steering state between operational plans
149 * @param points Set<DirectedPoint>; reference points
150 * @param segments List<Segment>; speed segments
151 * @return operational plan
152 * @throws ParameterException undefined parameter
153 */
154 public static OperationalPlan fromReferencePoints(final GTU gtu, final Parameters params, final SteeringState steeringState,
155 final Set<DirectedPoint> points, final List<Segment> segments) throws ParameterException
156 {
157 // TODO: implement steering control based on reference points
158 throw new UnsupportedOperationException();
159 }
160
161 /**
162 * Object that stores the information the steering utility requires.
163 * <p>
164 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
165 * <br>
166 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
167 * <p>
168 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 8 jan. 2019 <br>
169 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
170 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
171 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
172 */
173 public static class SteeringState
174 {
175
176 /** Steering angle; this is the angle of the front wheels relative to the vehicle longitudinal axis. */
177 private Angle steeringAngle = Angle.ZERO;
178
179 /** Steering angle error. */
180 private Angle angularError = Angle.ZERO;
181
182 /** Steering angle error derivative. */
183 private double angularErrorDerivative = 0.0;
184
185 /** Position error. */
186 private Length positionError = Length.ZERO;
187
188 /** Position error derivative. */
189 private Speed positionErrorDerivative = Speed.ZERO;
190
191 /**
192 * @return steeringAngle.
193 */
194 protected Angle getSteeringAngle()
195 {
196 return this.steeringAngle;
197 }
198
199 /**
200 * @param steeringAngle Angle; set steeringAngle.
201 */
202 protected void setSteeringAngle(final Angle steeringAngle)
203 {
204 this.steeringAngle = steeringAngle;
205 }
206
207 /**
208 * @return angularError.
209 */
210 protected Angle getAngularError()
211 {
212 return this.angularError;
213 }
214
215 /**
216 * @param angularError Angle; set angularError.
217 */
218 protected void setAngularError(final Angle angularError)
219 {
220 this.angularError = angularError;
221 }
222
223 /**
224 * @return angularErrorDerivative.
225 */
226 protected double getAngularErrorDerivative()
227 {
228 return this.angularErrorDerivative;
229 }
230
231 /**
232 * @param angularErrorDerivative double; set angularErrorDerivative.
233 */
234 protected void setAngularErrorDerivative(final double angularErrorDerivative)
235 {
236 this.angularErrorDerivative = angularErrorDerivative;
237 }
238
239 /**
240 * @return positionError.
241 */
242 protected Length getPositionError()
243 {
244 return this.positionError;
245 }
246
247 /**
248 * @param positionError Length; set positionError.
249 */
250 protected void setPositionError(final Length positionError)
251 {
252 this.positionError = positionError;
253 }
254
255 /**
256 * @return positionErrorDerivative.
257 */
258 protected Speed getPositionErrorDerivative()
259 {
260 return this.positionErrorDerivative;
261 }
262
263 /**
264 * @param positionErrorDerivative Speed; set positionErrorDerivative.
265 */
266 protected void setPositionErrorDerivative(final Speed positionErrorDerivative)
267 {
268 this.positionErrorDerivative = positionErrorDerivative;
269 }
270
271 }
272
273 /**
274 * Class containing feedback values for curvature determination.
275 * <p>
276 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
277 * <br>
278 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
279 * <p>
280 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 8 jan. 2019 <br>
281 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
282 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
283 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
284 */
285 public static class FeedbackTable
286 {
287
288 /** Feedback vector per speed. */
289 private final List<FeedbackVector> feedbackVectors;
290
291 /**
292 * @param feedbackVectors List<FeedbackVector>; feedback vector per speed
293 */
294 public FeedbackTable(final List<FeedbackVector> feedbackVectors)
295 {
296 Throw.when(feedbackVectors == null || feedbackVectors.size() == 0, IllegalArgumentException.class,
297 "At least one feedback vector should be defined.");
298 this.feedbackVectors = feedbackVectors;
299 }
300
301 /**
302 * Returns the feedback vector pertaining to the speed closest to the input speed.
303 * @param speed Speed; speed
304 * @return feedback vector pertaining to the speed closest to the input speed
305 */
306 protected FeedbackVector getAngularErrorFeedback(final Speed speed)
307 {
308 FeedbackVector feedbackVector = null;
309 Speed deviation = Speed.POSITIVE_INFINITY;
310 for (int i = 0; i < this.feedbackVectors.size(); i++)
311 {
312 Speed dev = speed.minus(this.feedbackVectors.get(i).getSpeed()).abs();
313 if (dev.lt(deviation))
314 {
315 feedbackVector = this.feedbackVectors.get(i);
316 deviation = dev;
317 }
318 }
319 return feedbackVector;
320 }
321
322 /**
323 * Feedback value for a specific speed.
324 * <p>
325 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights
326 * reserved. <br>
327 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
328 * <p>
329 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 8 jan. 2019 <br>
330 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
331 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
332 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
333 */
334 public static class FeedbackVector
335 {
336
337 /** Speed. */
338 private final Speed speed;
339
340 /** Angular error feedback. */
341 private final double angularErrorFeedback;
342
343 /** Angular error derivative feedback. */
344 private final double angularErrorDerivateFeedback;
345
346 /** Position error feedback. */
347 private final double positionErrorFeedback;
348
349 /** Position error derivative feedback. */
350 private final double positionErrorDerivativeFeedback;
351
352 /**
353 * @param speed Speed; speed
354 * @param angularErrorFeedback double; angular error feedback
355 * @param angularErrorDerivateFeedback double; angular error derivative feedback
356 * @param positionErrorFeedback double; position error feedback
357 * @param positionErrorDerivativeFeedback double; position error derivative feedback
358 */
359 public FeedbackVector(final Speed speed, final double angularErrorFeedback,
360 final double angularErrorDerivateFeedback, final double positionErrorFeedback,
361 final double positionErrorDerivativeFeedback)
362 {
363 this.speed = speed;
364 this.angularErrorFeedback = angularErrorFeedback;
365 this.angularErrorDerivateFeedback = angularErrorDerivateFeedback;
366 this.positionErrorFeedback = positionErrorFeedback;
367 this.positionErrorDerivativeFeedback = positionErrorDerivativeFeedback;
368 }
369
370 /**
371 * @return speed.
372 */
373 protected Speed getSpeed()
374 {
375 return this.speed;
376 }
377
378 /**
379 * @return angularErrorFeedback.
380 */
381 protected double getAngularErrorFeedback()
382 {
383 return this.angularErrorFeedback;
384 }
385
386 /**
387 * @return angularErrorDerivateFeedback.
388 */
389 protected double getAngularErrorDerivateFeedback()
390 {
391 return this.angularErrorDerivateFeedback;
392 }
393
394 /**
395 * @return positionErrorFeedback.
396 */
397 protected double getPositionErrorFeedback()
398 {
399 return this.positionErrorFeedback;
400 }
401
402 /**
403 * @return positionErrorDerivativeFeedback.
404 */
405 protected double getPositionErrorDerivativeFeedback()
406 {
407 return this.positionErrorDerivativeFeedback;
408 }
409 }
410
411 }
412
413 }