001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005/* 006 * MIT License 007 * 008 * Copyright (c) 2018 Team 254 009 * 010 * Permission is hereby granted, free of charge, to any person obtaining a copy 011 * of this software and associated documentation files (the "Software"), to deal 012 * in the Software without restriction, including without limitation the rights 013 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 014 * copies of the Software, and to permit persons to whom the Software is 015 * furnished to do so, subject to the following conditions: 016 * 017 * The above copyright notice and this permission notice shall be included in all 018 * copies or substantial portions of the Software. 019 * 020 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 021 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 022 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 023 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 024 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 025 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 026 * SOFTWARE. 027 */ 028 029package edu.wpi.first.math.trajectory; 030 031import edu.wpi.first.math.spline.PoseWithCurvature; 032import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; 033import java.util.ArrayList; 034import java.util.List; 035 036/** Class used to parameterize a trajectory by time. */ 037public final class TrajectoryParameterizer { 038 /** Private constructor because this is a utility class. */ 039 private TrajectoryParameterizer() {} 040 041 /** 042 * Parameterize the trajectory by time. This is where the velocity profile is generated. 043 * 044 * <p>The derivation of the algorithm used can be found <a 045 * href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">here</a>. 046 * 047 * @param points Reference to the spline points. 048 * @param constraints A vector of various velocity and acceleration. constraints. 049 * @param startVelocity The start velocity for the trajectory in m/s. 050 * @param endVelocity The end velocity for the trajectory in m/s. 051 * @param maxVelocity The max velocity for the trajectory in m/s. 052 * @param maxAcceleration The max acceleration for the trajectory in m/s². 053 * @param reversed Whether the robot should move backwards. Note that the robot will still move 054 * from a -> b -> ... -> z as defined in the waypoints. 055 * @return The trajectory. 056 */ 057 public static Trajectory timeParameterizeTrajectory( 058 List<PoseWithCurvature> points, 059 List<TrajectoryConstraint> constraints, 060 double startVelocity, 061 double endVelocity, 062 double maxVelocity, 063 double maxAcceleration, 064 boolean reversed) { 065 var constrainedStates = new ArrayList<ConstrainedState>(points.size()); 066 var predecessor = 067 new ConstrainedState(points.get(0), 0, startVelocity, -maxAcceleration, maxAcceleration); 068 069 // Forward pass 070 for (int i = 0; i < points.size(); i++) { 071 constrainedStates.add(new ConstrainedState()); 072 var constrainedState = constrainedStates.get(i); 073 constrainedState.pose = points.get(i); 074 075 // Begin constraining based on predecessor. 076 double ds = 077 constrainedState 078 .pose 079 .pose 080 .getTranslation() 081 .getDistance(predecessor.pose.pose.getTranslation()); 082 constrainedState.distance = predecessor.distance + ds; 083 084 // We may need to iterate to find the maximum end velocity and common 085 // acceleration, since acceleration limits may be a function of velocity. 086 while (true) { 087 // Enforce global max velocity and max reachable velocity by global 088 // acceleration limit. v_f = √(v_i² + 2ad). 089 constrainedState.maxVelocity = 090 Math.min( 091 maxVelocity, 092 Math.sqrt( 093 predecessor.maxVelocity * predecessor.maxVelocity 094 + predecessor.maxAcceleration * ds * 2.0)); 095 096 constrainedState.minAcceleration = -maxAcceleration; 097 constrainedState.maxAcceleration = maxAcceleration; 098 099 // At this point, the constrained state is fully constructed apart from 100 // all the custom-defined user constraints. 101 for (final var constraint : constraints) { 102 constrainedState.maxVelocity = 103 Math.min( 104 constrainedState.maxVelocity, 105 constraint.getMaxVelocity( 106 constrainedState.pose.pose, 107 constrainedState.pose.curvature, 108 constrainedState.maxVelocity)); 109 } 110 111 // Now enforce all acceleration limits. 112 enforceAccelerationLimits(reversed, constraints, constrainedState); 113 114 if (ds < 1E-6) { 115 break; 116 } 117 118 // If the actual acceleration for this state is higher than the max 119 // acceleration that we applied, then we need to reduce the max 120 // acceleration of the predecessor and try again. 121 double actualAcceleration = 122 (constrainedState.maxVelocity * constrainedState.maxVelocity 123 - predecessor.maxVelocity * predecessor.maxVelocity) 124 / (ds * 2.0); 125 126 // If we violate the max acceleration constraint, let's modify the 127 // predecessor. 128 if (constrainedState.maxAcceleration < actualAcceleration - 1E-6) { 129 predecessor.maxAcceleration = constrainedState.maxAcceleration; 130 } else { 131 // Constrain the predecessor's max acceleration to the current 132 // acceleration. 133 if (actualAcceleration > predecessor.minAcceleration) { 134 predecessor.maxAcceleration = actualAcceleration; 135 } 136 // If the actual acceleration is less than the predecessor's min 137 // acceleration, it will be repaired in the backward pass. 138 break; 139 } 140 } 141 predecessor = constrainedState; 142 } 143 144 var successor = 145 new ConstrainedState( 146 points.get(points.size() - 1), 147 constrainedStates.get(constrainedStates.size() - 1).distance, 148 endVelocity, 149 -maxAcceleration, 150 maxAcceleration); 151 152 // Backward pass 153 for (int i = points.size() - 1; i >= 0; i--) { 154 var constrainedState = constrainedStates.get(i); 155 double ds = constrainedState.distance - successor.distance; // negative 156 157 while (true) { 158 // Enforce max velocity limit (reverse) 159 // v_f = √(v_i² + 2ad), where v_i = successor. 160 double newMaxVelocity = 161 Math.sqrt( 162 successor.maxVelocity * successor.maxVelocity 163 + successor.minAcceleration * ds * 2.0); 164 165 // No more limits to impose! This state can be finalized. 166 if (newMaxVelocity >= constrainedState.maxVelocity) { 167 break; 168 } 169 170 constrainedState.maxVelocity = newMaxVelocity; 171 172 // Check all acceleration constraints with the new max velocity. 173 enforceAccelerationLimits(reversed, constraints, constrainedState); 174 175 if (ds > -1E-6) { 176 break; 177 } 178 179 // If the actual acceleration for this state is lower than the min 180 // acceleration, then we need to lower the min acceleration of the 181 // successor and try again. 182 double actualAcceleration = 183 (constrainedState.maxVelocity * constrainedState.maxVelocity 184 - successor.maxVelocity * successor.maxVelocity) 185 / (ds * 2.0); 186 187 if (constrainedState.minAcceleration > actualAcceleration + 1E-6) { 188 successor.minAcceleration = constrainedState.minAcceleration; 189 } else { 190 successor.minAcceleration = actualAcceleration; 191 break; 192 } 193 } 194 successor = constrainedState; 195 } 196 197 // Now we can integrate the constrained states forward in time to obtain our 198 // trajectory states. 199 var states = new ArrayList<Trajectory.State>(points.size()); 200 double time = 0.0; 201 double distance = 0.0; 202 double velocity = 0.0; 203 204 for (int i = 0; i < constrainedStates.size(); i++) { 205 final var state = constrainedStates.get(i); 206 207 // Calculate the change in position between the current state and the previous 208 // state. 209 double ds = state.distance - distance; 210 211 // Calculate the acceleration between the current state and the previous 212 // state. 213 double accel = (state.maxVelocity * state.maxVelocity - velocity * velocity) / (ds * 2); 214 215 // Calculate dt 216 double dt = 0.0; 217 if (i > 0) { 218 states.get(i - 1).acceleration = reversed ? -accel : accel; 219 if (Math.abs(accel) > 1E-6) { 220 // v_f = v_0 + a * t 221 dt = (state.maxVelocity - velocity) / accel; 222 } else if (Math.abs(velocity) > 1E-6) { 223 // delta_x = v * t 224 dt = ds / velocity; 225 } else { 226 throw new TrajectoryGenerationException( 227 "Something went wrong at iteration " + i + " of time parameterization."); 228 } 229 } 230 231 velocity = state.maxVelocity; 232 distance = state.distance; 233 234 time += dt; 235 236 states.add( 237 new Trajectory.State( 238 time, 239 reversed ? -velocity : velocity, 240 reversed ? -accel : accel, 241 state.pose.pose, 242 state.pose.curvature)); 243 } 244 245 return new Trajectory(states); 246 } 247 248 private static void enforceAccelerationLimits( 249 boolean reverse, List<TrajectoryConstraint> constraints, ConstrainedState state) { 250 for (final var constraint : constraints) { 251 double factor = reverse ? -1.0 : 1.0; 252 final var minMaxAccel = 253 constraint.getMinMaxAcceleration( 254 state.pose.pose, state.pose.curvature, state.maxVelocity * factor); 255 256 if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) { 257 throw new TrajectoryGenerationException( 258 "Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n"); 259 } 260 261 state.minAcceleration = 262 Math.max( 263 state.minAcceleration, 264 reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration); 265 266 state.maxAcceleration = 267 Math.min( 268 state.maxAcceleration, 269 reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration); 270 } 271 } 272 273 private static class ConstrainedState { 274 PoseWithCurvature pose; 275 double distance; 276 double maxVelocity; 277 double minAcceleration; 278 double maxAcceleration; 279 280 ConstrainedState( 281 PoseWithCurvature pose, 282 double distance, 283 double maxVelocity, 284 double minAcceleration, 285 double maxAcceleration) { 286 this.pose = pose; 287 this.distance = distance; 288 this.maxVelocity = maxVelocity; 289 this.minAcceleration = minAcceleration; 290 this.maxAcceleration = maxAcceleration; 291 } 292 293 ConstrainedState() { 294 pose = new PoseWithCurvature(); 295 } 296 } 297 298 /** Exception for trajectory generation failure. */ 299 public static class TrajectoryGenerationException extends RuntimeException { 300 /** 301 * Constructs a TrajectoryGenerationException. 302 * 303 * @param message Exception message. 304 */ 305 public TrajectoryGenerationException(String message) { 306 super(message); 307 } 308 } 309}