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 startVelocityMetersPerSecond The start velocity for the trajectory. 050 * @param endVelocityMetersPerSecond The end velocity for the trajectory. 051 * @param maxVelocityMetersPerSecond The max velocity for the trajectory. 052 * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. 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 startVelocityMetersPerSecond, 061 double endVelocityMetersPerSecond, 062 double maxVelocityMetersPerSecond, 063 double maxAccelerationMetersPerSecondSq, 064 boolean reversed) { 065 var constrainedStates = new ArrayList<ConstrainedState>(points.size()); 066 var predecessor = 067 new ConstrainedState( 068 points.get(0), 069 0, 070 startVelocityMetersPerSecond, 071 -maxAccelerationMetersPerSecondSq, 072 maxAccelerationMetersPerSecondSq); 073 074 // Forward pass 075 for (int i = 0; i < points.size(); i++) { 076 constrainedStates.add(new ConstrainedState()); 077 var constrainedState = constrainedStates.get(i); 078 constrainedState.pose = points.get(i); 079 080 // Begin constraining based on predecessor. 081 double ds = 082 constrainedState 083 .pose 084 .poseMeters 085 .getTranslation() 086 .getDistance(predecessor.pose.poseMeters.getTranslation()); 087 constrainedState.distanceMeters = predecessor.distanceMeters + ds; 088 089 // We may need to iterate to find the maximum end velocity and common 090 // acceleration, since acceleration limits may be a function of velocity. 091 while (true) { 092 // Enforce global max velocity and max reachable velocity by global 093 // acceleration limit. v_f = √(v_i² + 2ad). 094 constrainedState.maxVelocityMetersPerSecond = 095 Math.min( 096 maxVelocityMetersPerSecond, 097 Math.sqrt( 098 predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond 099 + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)); 100 101 constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq; 102 constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; 103 104 // At this point, the constrained state is fully constructed apart from 105 // all the custom-defined user constraints. 106 for (final var constraint : constraints) { 107 constrainedState.maxVelocityMetersPerSecond = 108 Math.min( 109 constrainedState.maxVelocityMetersPerSecond, 110 constraint.getMaxVelocityMetersPerSecond( 111 constrainedState.pose.poseMeters, 112 constrainedState.pose.curvatureRadPerMeter, 113 constrainedState.maxVelocityMetersPerSecond)); 114 } 115 116 // Now enforce all acceleration limits. 117 enforceAccelerationLimits(reversed, constraints, constrainedState); 118 119 if (ds < 1E-6) { 120 break; 121 } 122 123 // If the actual acceleration for this state is higher than the max 124 // acceleration that we applied, then we need to reduce the max 125 // acceleration of the predecessor and try again. 126 double actualAcceleration = 127 (constrainedState.maxVelocityMetersPerSecond 128 * constrainedState.maxVelocityMetersPerSecond 129 - predecessor.maxVelocityMetersPerSecond 130 * predecessor.maxVelocityMetersPerSecond) 131 / (ds * 2.0); 132 133 // If we violate the max acceleration constraint, let's modify the 134 // predecessor. 135 if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) { 136 predecessor.maxAccelerationMetersPerSecondSq = 137 constrainedState.maxAccelerationMetersPerSecondSq; 138 } else { 139 // Constrain the predecessor's max acceleration to the current 140 // acceleration. 141 if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) { 142 predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration; 143 } 144 // If the actual acceleration is less than the predecessor's min 145 // acceleration, it will be repaired in the backward pass. 146 break; 147 } 148 } 149 predecessor = constrainedState; 150 } 151 152 var successor = 153 new ConstrainedState( 154 points.get(points.size() - 1), 155 constrainedStates.get(constrainedStates.size() - 1).distanceMeters, 156 endVelocityMetersPerSecond, 157 -maxAccelerationMetersPerSecondSq, 158 maxAccelerationMetersPerSecondSq); 159 160 // Backward pass 161 for (int i = points.size() - 1; i >= 0; i--) { 162 var constrainedState = constrainedStates.get(i); 163 double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative 164 165 while (true) { 166 // Enforce max velocity limit (reverse) 167 // v_f = √(v_i² + 2ad), where v_i = successor. 168 double newMaxVelocity = 169 Math.sqrt( 170 successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond 171 + successor.minAccelerationMetersPerSecondSq * ds * 2.0); 172 173 // No more limits to impose! This state can be finalized. 174 if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) { 175 break; 176 } 177 178 constrainedState.maxVelocityMetersPerSecond = newMaxVelocity; 179 180 // Check all acceleration constraints with the new max velocity. 181 enforceAccelerationLimits(reversed, constraints, constrainedState); 182 183 if (ds > -1E-6) { 184 break; 185 } 186 187 // If the actual acceleration for this state is lower than the min 188 // acceleration, then we need to lower the min acceleration of the 189 // successor and try again. 190 double actualAcceleration = 191 (constrainedState.maxVelocityMetersPerSecond 192 * constrainedState.maxVelocityMetersPerSecond 193 - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond) 194 / (ds * 2.0); 195 196 if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) { 197 successor.minAccelerationMetersPerSecondSq = 198 constrainedState.minAccelerationMetersPerSecondSq; 199 } else { 200 successor.minAccelerationMetersPerSecondSq = actualAcceleration; 201 break; 202 } 203 } 204 successor = constrainedState; 205 } 206 207 // Now we can integrate the constrained states forward in time to obtain our 208 // trajectory states. 209 var states = new ArrayList<Trajectory.State>(points.size()); 210 double timeSeconds = 0.0; 211 double distanceMeters = 0.0; 212 double velocityMetersPerSecond = 0.0; 213 214 for (int i = 0; i < constrainedStates.size(); i++) { 215 final var state = constrainedStates.get(i); 216 217 // Calculate the change in position between the current state and the previous 218 // state. 219 double ds = state.distanceMeters - distanceMeters; 220 221 // Calculate the acceleration between the current state and the previous 222 // state. 223 double accel = 224 (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond 225 - velocityMetersPerSecond * velocityMetersPerSecond) 226 / (ds * 2); 227 228 // Calculate dt 229 double dt = 0.0; 230 if (i > 0) { 231 states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel; 232 if (Math.abs(accel) > 1E-6) { 233 // v_f = v_0 + a * t 234 dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel; 235 } else if (Math.abs(velocityMetersPerSecond) > 1E-6) { 236 // delta_x = v * t 237 dt = ds / velocityMetersPerSecond; 238 } else { 239 throw new TrajectoryGenerationException( 240 "Something went wrong at iteration " + i + " of time parameterization."); 241 } 242 } 243 244 velocityMetersPerSecond = state.maxVelocityMetersPerSecond; 245 distanceMeters = state.distanceMeters; 246 247 timeSeconds += dt; 248 249 states.add( 250 new Trajectory.State( 251 timeSeconds, 252 reversed ? -velocityMetersPerSecond : velocityMetersPerSecond, 253 reversed ? -accel : accel, 254 state.pose.poseMeters, 255 state.pose.curvatureRadPerMeter)); 256 } 257 258 return new Trajectory(states); 259 } 260 261 private static void enforceAccelerationLimits( 262 boolean reverse, List<TrajectoryConstraint> constraints, ConstrainedState state) { 263 for (final var constraint : constraints) { 264 double factor = reverse ? -1.0 : 1.0; 265 final var minMaxAccel = 266 constraint.getMinMaxAccelerationMetersPerSecondSq( 267 state.pose.poseMeters, 268 state.pose.curvatureRadPerMeter, 269 state.maxVelocityMetersPerSecond * factor); 270 271 if (minMaxAccel.minAccelerationMetersPerSecondSq 272 > minMaxAccel.maxAccelerationMetersPerSecondSq) { 273 throw new TrajectoryGenerationException( 274 "Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n"); 275 } 276 277 state.minAccelerationMetersPerSecondSq = 278 Math.max( 279 state.minAccelerationMetersPerSecondSq, 280 reverse 281 ? -minMaxAccel.maxAccelerationMetersPerSecondSq 282 : minMaxAccel.minAccelerationMetersPerSecondSq); 283 284 state.maxAccelerationMetersPerSecondSq = 285 Math.min( 286 state.maxAccelerationMetersPerSecondSq, 287 reverse 288 ? -minMaxAccel.minAccelerationMetersPerSecondSq 289 : minMaxAccel.maxAccelerationMetersPerSecondSq); 290 } 291 } 292 293 private static class ConstrainedState { 294 PoseWithCurvature pose; 295 double distanceMeters; 296 double maxVelocityMetersPerSecond; 297 double minAccelerationMetersPerSecondSq; 298 double maxAccelerationMetersPerSecondSq; 299 300 ConstrainedState( 301 PoseWithCurvature pose, 302 double distanceMeters, 303 double maxVelocityMetersPerSecond, 304 double minAccelerationMetersPerSecondSq, 305 double maxAccelerationMetersPerSecondSq) { 306 this.pose = pose; 307 this.distanceMeters = distanceMeters; 308 this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond; 309 this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq; 310 this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; 311 } 312 313 ConstrainedState() { 314 pose = new PoseWithCurvature(); 315 } 316 } 317 318 /** Exception for trajectory generation failure. */ 319 public static class TrajectoryGenerationException extends RuntimeException { 320 /** 321 * Constructs a TrajectoryGenerationException. 322 * 323 * @param message Exception message. 324 */ 325 public TrajectoryGenerationException(String message) { 326 super(message); 327 } 328 } 329}