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