View Javadoc
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&lt;DirectedPoint&gt;; reference points
149      * @param segments List&lt;Segment&gt;; 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&lt;FeedbackVector&gt;; 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 }