1 package org.opentrafficsim.core.gtu.plan.operational;
2
3 import java.util.ArrayList;
4
5 import org.djunits.unit.AccelerationUnit;
6 import org.djunits.unit.DurationUnit;
7 import org.djunits.unit.SpeedUnit;
8 import org.djunits.value.ValueException;
9 import org.djunits.value.vdouble.scalar.Acceleration;
10 import org.djunits.value.vdouble.scalar.Duration;
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.djunits.value.vdouble.scalar.Speed;
13 import org.djunits.value.vdouble.scalar.Time;
14 import org.opentrafficsim.core.geometry.OTSGeometryException;
15 import org.opentrafficsim.core.geometry.OTSLine3D;
16 import org.opentrafficsim.core.geometry.OTSPoint3D;
17 import org.opentrafficsim.core.gtu.GTU;
18 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
19 import org.opentrafficsim.core.math.Solver;
20
21 /**
22 * Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
23 * to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
24 * speed at the end of a curve, etc.<br>
25 * TODO driving with negative speeds (backward driving) is not yet supported.<br>
26 * TODO plan with a constant speed.
27 * <p>
28 * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
29 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
30 * </p>
31 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
32 * initial version Nov 15, 2015 <br>
33 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
34 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
35 */
36 public final class OperationalPlanBuilder
37 {
38 /** The maximum acceleration for unbounded accelerations: 1E12 m/s2. */
39 private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);
40
41 /** The maximum deceleration for unbounded accelerations: -1E12 m/s2. */
42 private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);
43
44 /** Private constructor prevents instantiation. */
45 private OperationalPlanBuilder()
46 {
47 // class should not be instantiated
48 }
49
50 /**
51 * Build a plan with a path and a given speed.
52 * @param gtu GTU; the GTU for debugging purposes
53 * @param path OTSLine3D; the path to drive (provides the length)
54 * @param startTime Time; the current time or a time in the future when the plan should start
55 * @param speed Speed; the speed at the start of the path
56 * @return the operational plan to accomplish the given end speed
57 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
58 * constructed segment list differ more than a given threshold
59 */
60 public static OperationalPlan buildConstantSpeedPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
61 final Speed speed) throws OperationalPlanException
62 {
63 Length length = path.getLength();
64 OperationalPlan.Segment segment;
65 segment = new SpeedSegment(length.divideBy(speed));
66 ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
67 segmentList.add(segment);
68 return new OperationalPlan(gtu, path, startTime, speed, segmentList);
69 }
70
71 /**
72 * Build a plan with a path and a given start speed to try to reach a provided end speed, exactly at the end of the curve.
73 * The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore, there is no guarantee
74 * that the end speed is actually reached by this plan.
75 * @param gtu GTU; the GTU for debugging purposes
76 * @param path OTSLine3D; the path to drive (provides the length)
77 * @param startTime Time; the current time or a time in the future when the plan should start
78 * @param startSpeed Speed; the speed at the start of the path
79 * @param endSpeed Speed; the required end speed
80 * @param maxAcceleration Acceleration; the maximum acceleration that can be applied, provided as a POSITIVE number
81 * @param maxDeceleration Acceleration; the maximum deceleration that can be applied, provided as a NEGATIVE number
82 * @return the operational plan to accomplish the given end speed
83 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
84 * constructed segment list differ more than a given threshold
85 */
86 public static OperationalPlan buildGradualAccelerationPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
87 final Speed startSpeed, final Speed endSpeed, final Acceleration maxAcceleration,
88 final Acceleration maxDeceleration) throws OperationalPlanException
89 {
90 Length length = path.getLength();
91 OperationalPlan.Segment segment;
92 if (startSpeed.eq(endSpeed))
93 {
94 segment = new SpeedSegment(length.divideBy(startSpeed));
95 }
96 else
97 {
98 // t = 2x / (vt + v0); a = (vt - v0) / t
99 Duration duration = length.multiplyBy(2.0).divideBy(endSpeed.plus(startSpeed));
100 Acceleration acceleration = endSpeed.minus(startSpeed).divideBy(duration);
101 try
102 {
103 if (acceleration.si < 0.0 && acceleration.lt(maxDeceleration))
104 {
105 acceleration = maxDeceleration;
106 // duration = new Duration(abc(acceleration.si / 2, startSpeed.si, -length.si), DurationUnit.SI);
107 duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -length.si),
108 DurationUnit.SI);
109 }
110 if (acceleration.si > 0.0 && acceleration.gt(maxAcceleration))
111 {
112 acceleration = maxAcceleration;
113 // duration = new Duration(abc(acceleration.si / 2, startSpeed.si, -length.si), DurationUnit.SI);
114 duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -length.si),
115 DurationUnit.SI);
116 }
117 }
118 catch (ValueException exception)
119 {
120 throw new OperationalPlanException("Caught unexpected exception: " + exception);
121 }
122 segment = new OperationalPlan.AccelerationSegment(duration, acceleration);
123 }
124 ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
125 segmentList.add(segment);
126 return new OperationalPlan(gtu, path, startTime, startSpeed, segmentList);
127 }
128
129 /**
130 * Build a plan with a path and a given start speed to reach a provided end speed, exactly at the end of the curve.
131 * Acceleration and deceleration are virtually unbounded (1E12 m/s2) to reach the end speed (e.g., to come to a complete
132 * stop).
133 * @param gtu GTU; the GTU for debugging purposes
134 * @param path OTSLine3D; the path to drive (provides the length)
135 * @param startTime Time; the current time or a time in the future when the plan should start
136 * @param startSpeed Speed; the speed at the start of the path
137 * @param endSpeed Speed; the required end speed
138 * @return the operational plan to accomplish the given end speed
139 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
140 * constructed segment list differ more than a given threshold
141 */
142 public static OperationalPlan buildGradualAccelerationPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
143 final Speed startSpeed, final Speed endSpeed) throws OperationalPlanException
144 {
145 return buildGradualAccelerationPlan(gtu, path, startTime, startSpeed, endSpeed, MAX_ACCELERATION, MAX_DECELERATION);
146 }
147
148 /**
149 * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
150 * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
151 * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
152 * before completing the path, a truncated path that ends where the GTU stops is used instead.
153 * @param gtu GTU; the GTU for debugging purposes
154 * @param path OTSLine3D; the path to drive (provides the length)
155 * @param startTime Time; the current time or a time in the future when the plan should start
156 * @param startSpeed Speed; the speed at the start of the path
157 * @param endSpeed Speed; the required end speed
158 * @param acceleration Acceleration; the acceleration to use if endSpeed > startSpeed, provided as a POSITIVE number
159 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
160 * @return the operational plan to accomplish the given end speed
161 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
162 * constructed segment list differ more than a given threshold
163 */
164 public static OperationalPlan buildMaximumAccelerationPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
165 final Speed startSpeed, final Speed endSpeed, final Acceleration acceleration, final Acceleration deceleration)
166 throws OperationalPlanException
167 {
168 Length length = path.getLength();
169 ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
170 if (startSpeed.eq(endSpeed))
171 {
172 segmentList.add(new OperationalPlan.SpeedSegment(length.divideBy(startSpeed)));
173 }
174 else
175 {
176 try
177 {
178 if (endSpeed.gt(startSpeed))
179 {
180 Duration t = endSpeed.minus(startSpeed).divideBy(acceleration);
181 Length x = startSpeed.multiplyBy(t).plus(acceleration.multiplyBy(0.5).multiplyBy(t).multiplyBy(t));
182 if (x.ge(length))
183 {
184 // we cannot reach the end speed in the given distance with the given acceleration
185 // Duration duration = new Duration(abc(acceleration.si / 2, startSpeed.si, -length.si),
186 // DurationUnit.SI);
187 Duration duration = new Duration(
188 Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -length.si), DurationUnit.SI);
189 segmentList.add(new OperationalPlan.AccelerationSegment(duration, acceleration));
190 }
191 else
192 {
193 // we reach the (higher) end speed before the end of the segment. Make two segments.
194 segmentList.add(new OperationalPlan.AccelerationSegment(t, acceleration));
195 Duration duration = length.minus(x).divideBy(endSpeed);
196 segmentList.add(new OperationalPlan.SpeedSegment(duration));
197 }
198 }
199 else
200 {
201 Duration t = endSpeed.minus(startSpeed).divideBy(deceleration);
202 Length x = startSpeed.multiplyBy(t).plus(deceleration.multiplyBy(0.5).multiplyBy(t).multiplyBy(t));
203 if (x.ge(length))
204 {
205 // we cannot reach the end speed in the given distance with the given deceleration
206 // Duration duration = new Duration(abc(deceleration.si / 2, startSpeed.si, -length.si),
207 // DurationUnit.SI);
208 Duration duration = new Duration(
209 Solver.firstSolutionAfter(0, deceleration.si / 2, startSpeed.si, -length.si), DurationUnit.SI);
210 segmentList.add(new OperationalPlan.AccelerationSegment(duration, deceleration));
211 }
212 else
213 {
214 if (endSpeed.si == 0.0)
215 {
216 // if endSpeed == 0, we cannot reach the end of the path. Therefore, build a partial path.
217 OTSLine3D partialPath = path.truncate(x.si);
218 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
219 return new OperationalPlan(gtu, partialPath, startTime, startSpeed, segmentList);
220 }
221 // we reach the (lower) end speed, larger than zero, before the end of the segment. Make two segments.
222 segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
223 Duration duration = length.minus(x).divideBy(endSpeed);
224 segmentList.add(new OperationalPlan.SpeedSegment(duration));
225 }
226 }
227 }
228 catch (ValueException | OTSGeometryException exception)
229 {
230 throw new OperationalPlanException("Caught unexpected exception: " + exception);
231 }
232 }
233 return new OperationalPlan(gtu, path, startTime, startSpeed, segmentList);
234 }
235
236 /**
237 * Build a plan with a path and a given start speed to try to come to a stop with a given deceleration. If the GTU can stop
238 * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
239 * that the OperationalPlan will lead to a complete stop.
240 * @param gtu GTU; the GTU for debugging purposes
241 * @param path OTSLine3D; the path to drive (provides the length)
242 * @param startTime Time; the current time or a time in the future when the plan should start
243 * @param startSpeed Speed; the speed at the start of the path
244 * @param deceleration Acceleration; the deceleration to use if endSpeed < startSpeed, provided as a NEGATIVE number
245 * @return the operational plan to accomplish the given end speed
246 * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
247 * constructed segment list differ more than a given threshold
248 */
249 public static OperationalPlan buildStopPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
250 final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException
251 {
252 return buildMaximumAccelerationPlan(gtu, path, startTime, startSpeed, Speed.ZERO,
253 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
254 }
255
256 /**
257 * Test.
258 * @param args String[]; args for main
259 * @throws OperationalPlanException on error
260 * @throws OTSGeometryException on error
261 */
262 public static void main(final String[] args) throws OperationalPlanException, OTSGeometryException
263 {
264 OTSLine3D path1 = new OTSLine3D(new OTSPoint3D[] {new OTSPoint3D(0.0, 0.0), new OTSPoint3D(100.0, 0.0)});
265
266 // go from 0 to 10 m/s over entire distance. This should take 20 sec with a=0.5 m/s2.
267 OperationalPlan plan1 =
268 buildGradualAccelerationPlan(null, path1, Time.ZERO, Speed.ZERO, new Speed(10.0, SpeedUnit.METER_PER_SECOND));
269 System.out.println(plan1);
270
271 // go from 0 to 10 m/s over entire distance, but limit a to 0.1 m/s2.
272 // This should take 44.72 sec with a=0.1 m/s2, and an end speed of 4.472 m/s.
273 OperationalPlan plan2 = buildGradualAccelerationPlan(null, path1, Time.ZERO, Speed.ZERO,
274 new Speed(10.0, SpeedUnit.METER_PER_SECOND), new Acceleration(0.1, AccelerationUnit.METER_PER_SECOND_2),
275 new Acceleration(-0.1, AccelerationUnit.METER_PER_SECOND_2));
276 System.out.println(plan2);
277
278 // go from 0 to 10 m/s with a = 1 m/s2, followed by a constant speed of 10 m/s.
279 // This should take 10 sec with a = 1 m/s2, reaching 50 m. After that, 50 m with 10 m/s in 5 sec.
280 OperationalPlan plan3 = buildMaximumAccelerationPlan(null, path1, Time.ZERO, Speed.ZERO,
281 new Speed(10.0, SpeedUnit.METER_PER_SECOND), new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2),
282 new Acceleration(-1.0, AccelerationUnit.METER_PER_SECOND_2));
283 System.out.println(plan3);
284
285 // go from 10 to 0 m/s with a = -1 m/s2, which should truncate the path at 50 m.
286 // This should take 10 sec with a = -1 m/s2, reaching 50 m. After that, the plan should stop.
287 OperationalPlan plan4 =
288 buildMaximumAccelerationPlan(null, path1, Time.ZERO, new Speed(10.0, SpeedUnit.METER_PER_SECOND),
289 new Speed(0.0, SpeedUnit.METER_PER_SECOND), new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2),
290 new Acceleration(-1.0, AccelerationUnit.METER_PER_SECOND_2));
291 System.out.println(plan4);
292
293 // try to stop with a = -2 m/s2, which should truncate the path at 25 m.
294 // This should take 5 sec with a = -2 m/s2, reaching 25 m. After that, the plan should stop.
295 OperationalPlan plan5 = buildStopPlan(null, path1, Time.ZERO, new Speed(10.0, SpeedUnit.METER_PER_SECOND),
296 new Acceleration(-2.0, AccelerationUnit.METER_PER_SECOND_2));
297 System.out.println(plan5);
298
299 }
300 }