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
30
31
32
33
34
35
36
37
38
39 public final class Steering
40 {
41
42
43 static final ParameterTypeDuration DT =
44 new ParameterTypeDuration("DTS", "Steering update time step", Duration.instantiateSI(0.01), NumericConstraint.POSITIVE);
45
46
47 static final ParameterTypeDouble C_FRONT = new ParameterTypeDouble("C_FRONT", "Front tire cornering stiffness", 80000);
48
49
50 static final ParameterTypeDouble C_REAR = new ParameterTypeDouble("C_REAR", "Rear tire cornering stiffness", 80000);
51
52
53
54
55 private Steering()
56 {
57
58 }
59
60
61
62
63
64
65
66
67
68
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);
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);
107 }
108
109
110 points.add(new OTSPoint3D(pos));
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
136
137 throw new RuntimeException(exception);
138 }
139
140 return referencePlan;
141 }
142
143
144
145
146
147
148
149
150
151
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
157 throw new UnsupportedOperationException();
158 }
159
160
161
162
163
164
165
166
167
168
169
170
171
172 public static class SteeringState
173 {
174
175
176 private Angle steeringAngle = Angle.ZERO;
177
178
179 private Angle angularError = Angle.ZERO;
180
181
182 private double angularErrorDerivative = 0.0;
183
184
185 private Length positionError = Length.ZERO;
186
187
188 private Speed positionErrorDerivative = Speed.ZERO;
189
190
191
192
193 protected Angle getSteeringAngle()
194 {
195 return this.steeringAngle;
196 }
197
198
199
200
201 protected void setSteeringAngle(final Angle steeringAngle)
202 {
203 this.steeringAngle = steeringAngle;
204 }
205
206
207
208
209 protected Angle getAngularError()
210 {
211 return this.angularError;
212 }
213
214
215
216
217 protected void setAngularError(final Angle angularError)
218 {
219 this.angularError = angularError;
220 }
221
222
223
224
225 protected double getAngularErrorDerivative()
226 {
227 return this.angularErrorDerivative;
228 }
229
230
231
232
233 protected void setAngularErrorDerivative(final double angularErrorDerivative)
234 {
235 this.angularErrorDerivative = angularErrorDerivative;
236 }
237
238
239
240
241 protected Length getPositionError()
242 {
243 return this.positionError;
244 }
245
246
247
248
249 protected void setPositionError(final Length positionError)
250 {
251 this.positionError = positionError;
252 }
253
254
255
256
257 protected Speed getPositionErrorDerivative()
258 {
259 return this.positionErrorDerivative;
260 }
261
262
263
264
265 protected void setPositionErrorDerivative(final Speed positionErrorDerivative)
266 {
267 this.positionErrorDerivative = positionErrorDerivative;
268 }
269
270 }
271
272
273
274
275
276
277
278
279
280
281
282
283
284 public static class FeedbackTable
285 {
286
287
288 private final List<FeedbackVector> feedbackVectors;
289
290
291
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
302
303
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
323
324
325
326
327
328
329
330
331
332
333 public static class FeedbackVector
334 {
335
336
337 private final Speed speed;
338
339
340 private final double angularErrorFeedback;
341
342
343 private final double angularErrorDerivateFeedback;
344
345
346 private final double positionErrorFeedback;
347
348
349 private final double positionErrorDerivativeFeedback;
350
351
352
353
354
355
356
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
371
372 protected Speed getSpeed()
373 {
374 return this.speed;
375 }
376
377
378
379
380 protected double getAngularErrorFeedback()
381 {
382 return this.angularErrorFeedback;
383 }
384
385
386
387
388 protected double getAngularErrorDerivateFeedback()
389 {
390 return this.angularErrorDerivateFeedback;
391 }
392
393
394
395
396 protected double getPositionErrorFeedback()
397 {
398 return this.positionErrorFeedback;
399 }
400
401
402
403
404 protected double getPositionErrorDerivativeFeedback()
405 {
406 return this.positionErrorDerivativeFeedback;
407 }
408 }
409
410 }
411
412 }