OperationalPlanBuilder.java

  1. package org.opentrafficsim.core.gtu.plan.operational;

  3. import java.util.ArrayList;

  5. import org.djunits.unit.AccelerationUnit;
  6. import org.djunits.unit.DurationUnit;
  7. import org.djunits.value.ValueRuntimeException;
  8. import org.djunits.value.vdouble.scalar.Acceleration;
  9. import org.djunits.value.vdouble.scalar.Duration;
  10. import org.djunits.value.vdouble.scalar.Length;
  11. import org.djunits.value.vdouble.scalar.Speed;
  12. import org.djunits.value.vdouble.scalar.Time;
  13. import org.opentrafficsim.core.geometry.OTSGeometryException;
  14. import org.opentrafficsim.core.geometry.OTSLine3D;
  15. import org.opentrafficsim.core.gtu.GTU;
  16. import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
  17. import org.opentrafficsim.core.math.Solver;

  19. /**
  20.  * Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
  21.  * to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
  22.  * speed at the end of a curve, etc.<br>
  23.  * TODO driving with negative speeds (backward driving) is not yet supported.<br>
  24.  * TODO plan with a constant speed.
  25.  * <p>
  26.  * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  27.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  28.  * </p>
  29.  * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
  30.  * initial version Nov 15, 2015 <br>
  31.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  32.  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  33.  */
  34. public final class OperationalPlanBuilder
  35. {
  36.     /** The maximum acceleration for unbounded accelerations: 1E12 m/s2. */
  37.     private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);

  39.     /** The maximum deceleration for unbounded accelerations: -1E12 m/s2. */
  40.     private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);

  42.     /** Private constructor prevents instantiation. */
  43.     private OperationalPlanBuilder()
  44.     {
  45.         // class should not be instantiated
  46.     }

  48.     /**
  49.      * Build a plan with a path and a given speed.
  50.      * @param gtu GTU; the GTU for debugging purposes
  51.      * @param path OTSLine3D; the path to drive (provides the length)
  52.      * @param startTime Time; the current time or a time in the future when the plan should start
  53.      * @param speed Speed; the speed at the start of the path
  54.      * @return the operational plan to accomplish the given end speed
  55.      * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
  56.      *             constructed segment list differ more than a given threshold
  57.      */
  58.     public static OperationalPlan buildConstantSpeedPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
  59.             final Speed speed) throws OperationalPlanException
  60.     {
  61.         Length length = path.getLength();
  62.         OperationalPlan.Segment segment;
  63.         segment = new SpeedSegment(length.divide(speed));
  64.         ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
  65.         segmentList.add(segment);
  66.         return new OperationalPlan(gtu, path, startTime, speed, segmentList);
  67.     }

  69.     /**
  70.      * Build a plan with a path and a given start speed to try to reach a provided end speed, exactly at the provided
  71.      * <code>endTime</code>. The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore,
  72.      * there is no guarantee that the end speed is actually reached by this plan. <p>
  73.      * TODO: rename this method buildConstantAccelerationPlan.
  74.      * @param gtu GTU; the GTU for debugging purposes
  75.      * @param path OTSLine3D; the path to drive (provides the length)
  76.      * @param startTime Time; the current time or a time in the future when the plan should start
  77.      * @param startSpeed Speed; the speed at the start of the path
  78.      * @param endSpeed Speed; the required end speed
  79.      * @param maximumAcceleration Acceleration; the maximum acceleration that can be applied, provided as a POSITIVE number
  80.      * @param maximumDeceleration Acceleration; the maximum deceleration that can be applied, provided as a NEGATIVE number
  81.      * @return the operational plan to accomplish the given end speed
  82.      * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
  83.      *             constructed segment list differ more than a given threshold
  84.      */
  85.     public static OperationalPlan buildGradualAccelerationPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
  86.             final Speed startSpeed, final Speed endSpeed, final Acceleration maximumAcceleration,
  87.             final Acceleration maximumDeceleration) throws OperationalPlanException
  88.     {
  89.         Length length = path.getLength();
  90.         OperationalPlan.Segment segment;
  91.         if (startSpeed.eq(endSpeed))
  92.         {
  93.             segment = new SpeedSegment(length.divide(startSpeed));
  94.         }
  95.         else
  96.         {
  97.             // t = 2x / (vt + v0); a = (vt - v0) / t
  98.             Duration duration = length.times(2.0).divide(endSpeed.plus(startSpeed));
  99.             Acceleration acceleration = endSpeed.minus(startSpeed).divide(duration);
  100.             try
  101.             {
  102.                 if (acceleration.si < 0.0 && acceleration.lt(maximumDeceleration))
  103.                 {
  104.                     acceleration = maximumDeceleration;
  105.                     // duration = new Duration(abc(acceleration.si / 2, startSpeed.si, -length.si), DurationUnit.SI);
  106.                     duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -length.si),
  107.                             DurationUnit.SI);
  108.                 }
  109.                 if (acceleration.si > 0.0 && acceleration.gt(maximumAcceleration))
  110.                 {
  111.                     acceleration = maximumAcceleration;
  112.                     // duration = new Duration(abc(acceleration.si / 2, startSpeed.si, -length.si), DurationUnit.SI);
  113.                     duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -length.si),
  114.                             DurationUnit.SI);
  115.                 }
  116.             }
  117.             catch (ValueRuntimeException exception)
  118.             {
  119.                 throw new OperationalPlanException("Caught unexpected exception: " + exception);
  120.             }
  121.             segment = new OperationalPlan.AccelerationSegment(duration, acceleration);
  122.         }
  123.         ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
  124.         segmentList.add(segment);
  125.         return new OperationalPlan(gtu, path, startTime, startSpeed, segmentList);
  126.     }

  128.     /**
  129.      * Build a plan with a path and a given start speed to reach a provided end speed, exactly at the end of the curve.
  130.      * Acceleration and deceleration are virtually unbounded (1E12 m/s2) to reach the end speed (e.g., to come to a complete
  131.      * stop).
  132.      * @param gtu GTU; the GTU for debugging purposes
  133.      * @param path OTSLine3D; the path to drive (provides the length)
  134.      * @param startTime Time; the current time or a time in the future when the plan should start
  135.      * @param startSpeed Speed; the speed at the start of the path
  136.      * @param endSpeed Speed; the required end speed
  137.      * @return the operational plan to accomplish the given end speed
  138.      * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
  139.      *             constructed segment list differ more than a given threshold
  140.      */
  141.     public static OperationalPlan buildGradualAccelerationPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
  142.             final Speed startSpeed, final Speed endSpeed) throws OperationalPlanException
  143.     {
  144.         return buildGradualAccelerationPlan(gtu, path, startTime, startSpeed, endSpeed, MAX_ACCELERATION, MAX_DECELERATION);
  145.     }

  147.     /**
  148.      * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
  149.      * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
  150.      * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
  151.      * before completing the path, a truncated path that ends where the GTU stops is used instead. The maximum acceleration and
  152.      * deceleration is limited by the provided values. If these prevent the <code>endSpeed</code> from being reached, the
  153.      * generated plan is a constant acceleration plan using the limiting value.
  154.      * @param gtu GTU; the GTU for debugging purposes
  155.      * @param path OTSLine3D; the path to drive (provides the length)
  156.      * @param startTime Time; the current time or a time in the future when the plan should start
  157.      * @param startSpeed Speed; the speed at the start of the path
  158.      * @param endSpeed Speed; the required end speed
  159.      * @param maximumAcceleration Acceleration; the acceleration to use if endSpeed &gt; startSpeed, provided as a POSITIVE
  160.      *            number
  161.      * @param maximumDeceleration Acceleration; the deceleration to use if endSpeed &lt; startSpeed, provided as a NEGATIVE
  162.      *            number
  163.      * @return the operational plan to accomplish the given end speed
  164.      * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
  165.      *             constructed segment list differ more than a given threshold
  166.      */
  167.     public static OperationalPlan buildMaximumAccelerationPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
  168.             final Speed startSpeed, final Speed endSpeed, final Acceleration maximumAcceleration,
  169.             final Acceleration maximumDeceleration) throws OperationalPlanException
  170.     {
  171.         Length length = path.getLength();
  172.         ArrayList<OperationalPlan.Segment> segmentList = new ArrayList<>();
  173.         if (startSpeed.eq(endSpeed))
  174.         {
  175.             segmentList.add(new OperationalPlan.SpeedSegment(length.divide(startSpeed)));
  176.         }
  177.         else
  178.         {
  179.             try
  180.             {
  181.                 if (endSpeed.gt(startSpeed))
  182.                 {
  183.                     Duration t = endSpeed.minus(startSpeed).divide(maximumAcceleration);
  184.                     Length x = startSpeed.times(t).plus(maximumAcceleration.times(0.5).times(t).times(t));
  185.                     if (x.ge(length))
  186.                     {
  187.                         // we cannot reach the end speed in the given distance with the given acceleration
  188.                         // Duration duration = new Duration(abc(acceleration.si / 2, startSpeed.si, -length.si),
  189.                         // DurationUnit.SI);
  190.                         Duration duration = new Duration(
  191.                                 Solver.firstSolutionAfter(0, maximumAcceleration.si / 2, startSpeed.si, -length.si),
  192.                                 DurationUnit.SI);
  193.                         segmentList.add(new OperationalPlan.AccelerationSegment(duration, maximumAcceleration));
  194.                     }
  195.                     else
  196.                     {
  197.                         // we reach the (higher) end speed before the end of the segment. Make two segments.
  198.                         segmentList.add(new OperationalPlan.AccelerationSegment(t, maximumAcceleration));
  199.                         Duration duration = length.minus(x).divide(endSpeed);
  200.                         segmentList.add(new OperationalPlan.SpeedSegment(duration));
  201.                     }
  202.                 }
  203.                 else
  204.                 {
  205.                     Duration t = endSpeed.minus(startSpeed).divide(maximumDeceleration);
  206.                     Length x = startSpeed.times(t).plus(maximumDeceleration.times(0.5).times(t).times(t));
  207.                     if (x.ge(length))
  208.                     {
  209.                         // we cannot reach the end speed in the given distance with the given deceleration
  210.                         // Duration duration = new Duration(abc(deceleration.si / 2, startSpeed.si, -length.si),
  211.                         // DurationUnit.SI);
  212.                         Duration duration =
  213.                                 new Duration(Solver.firstSolutionAfter(0, maximumDeceleration.si / 2, startSpeed.si, -length.si),
  214.                                         DurationUnit.SI);
  215.                         segmentList.add(new OperationalPlan.AccelerationSegment(duration, maximumDeceleration));
  216.                     }
  217.                     else
  218.                     {
  219.                         if (endSpeed.si == 0.0)
  220.                         {
  221.                             // if endSpeed == 0, we cannot reach the end of the path. Therefore, build a partial path.
  222.                             OTSLine3D partialPath = path.truncate(x.si);
  223.                             segmentList.add(new OperationalPlan.AccelerationSegment(t, maximumDeceleration));
  224.                             return new OperationalPlan(gtu, partialPath, startTime, startSpeed, segmentList);
  225.                         }
  226.                         // we reach the (lower) end speed, larger than zero, before the end of the segment. Make two segments.
  227.                         segmentList.add(new OperationalPlan.AccelerationSegment(t, maximumDeceleration));
  228.                         Duration duration = length.minus(x).divide(endSpeed);
  229.                         segmentList.add(new OperationalPlan.SpeedSegment(duration));
  230.                     }
  231.                 }
  232.             }
  233.             catch (ValueRuntimeException | OTSGeometryException exception)
  234.             {
  235.                 throw new OperationalPlanException("Caught unexpected exception: " + exception);
  236.             }
  237.         }
  238.         return new OperationalPlan(gtu, path, startTime, startSpeed, segmentList);
  239.     }

  241.     /**
  242.      * 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
  243.      * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
  244.      * that the OperationalPlan will lead to a complete stop.
  245.      * @param gtu GTU; the GTU for debugging purposes
  246.      * @param path OTSLine3D; the path to drive (provides the length)
  247.      * @param startTime Time; the current time or a time in the future when the plan should start
  248.      * @param startSpeed Speed; the speed at the start of the path
  249.      * @param deceleration Acceleration; the deceleration to use if endSpeed &lt; startSpeed, provided as a NEGATIVE number
  250.      * @return the operational plan to accomplish the given end speed
  251.      * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
  252.      *             constructed segment list differ more than a given threshold
  253.      */
  254.     public static OperationalPlan buildStopPlan(final GTU gtu, final OTSLine3D path, final Time startTime,
  255.             final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException
  256.     {
  257.         return buildMaximumAccelerationPlan(gtu, path, startTime, startSpeed, Speed.ZERO,
  258.                 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
  259.     }

  261. }
Created with JaCoCo 0.8.6.202009150832