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 005package edu.wpi.first.math.trajectory; 006 007import edu.wpi.first.math.MathSharedStore; 008import edu.wpi.first.math.MathUsageId; 009import edu.wpi.first.units.Measure; 010import edu.wpi.first.units.Unit; 011import edu.wpi.first.units.Velocity; 012import java.util.Objects; 013 014/** 015 * A trapezoid-shaped velocity profile. 016 * 017 * <p>While this class can be used for a profiled movement from start to finish, the intended usage 018 * is to filter a reference's dynamics based on trapezoidal velocity constraints. To compute the 019 * reference obeying this constraint, do the following. 020 * 021 * <p>Initialization: 022 * 023 * <pre><code> 024 * TrapezoidProfile.Constraints constraints = 025 * new TrapezoidProfile.Constraints(kMaxV, kMaxA); 026 * TrapezoidProfile.State previousProfiledReference = 027 * new TrapezoidProfile.State(initialReference, 0.0); 028 * TrapezoidProfile profile = new TrapezoidProfile(constraints); 029 * </code></pre> 030 * 031 * <p>Run on update: 032 * 033 * <pre><code> 034 * previousProfiledReference = 035 * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference); 036 * </code></pre> 037 * 038 * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled 039 * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged. 040 * 041 * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to 042 * determine when the profile has completed via `isFinished()`. 043 */ 044public class TrapezoidProfile { 045 // The direction of the profile, either 1 for forwards or -1 for inverted 046 private int m_direction; 047 048 private final Constraints m_constraints; 049 private State m_current; 050 private State m_goal; // TODO: Remove 051 private final boolean m_newAPI; // TODO: Remove 052 053 private double m_endAccel; 054 private double m_endFullSpeed; 055 private double m_endDeccel; 056 057 public static class Constraints { 058 public final double maxVelocity; 059 060 public final double maxAcceleration; 061 062 /** 063 * Construct constraints for a TrapezoidProfile. 064 * 065 * @param maxVelocity maximum velocity 066 * @param maxAcceleration maximum acceleration 067 */ 068 public Constraints(double maxVelocity, double maxAcceleration) { 069 this.maxVelocity = maxVelocity; 070 this.maxAcceleration = maxAcceleration; 071 MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1); 072 } 073 074 public <U extends Unit<U>> Constraints( 075 Measure<Velocity<U>> maxVelocity, Measure<Velocity<Velocity<U>>> maxAcceleration) { 076 this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude()); 077 } 078 } 079 080 public static class State { 081 public double position; 082 083 public double velocity; 084 085 public State() {} 086 087 public State(double position, double velocity) { 088 this.position = position; 089 this.velocity = velocity; 090 } 091 092 public <U extends Unit<U>> State(Measure<U> position, Measure<Velocity<U>> velocity) { 093 this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude()); 094 } 095 096 @Override 097 public boolean equals(Object other) { 098 if (other instanceof State) { 099 State rhs = (State) other; 100 return this.position == rhs.position && this.velocity == rhs.velocity; 101 } else { 102 return false; 103 } 104 } 105 106 @Override 107 public int hashCode() { 108 return Objects.hash(position, velocity); 109 } 110 } 111 112 /** 113 * Construct a TrapezoidProfile. 114 * 115 * @param constraints The constraints on the profile, like maximum velocity. 116 */ 117 public TrapezoidProfile(Constraints constraints) { 118 m_constraints = constraints; 119 m_newAPI = true; 120 } 121 122 /** 123 * Construct a TrapezoidProfile. 124 * 125 * @param constraints The constraints on the profile, like maximum velocity. 126 * @param goal The desired state when the profile is complete. 127 * @param initial The initial state (usually the current state). 128 * @deprecated Pass the desired and current state into calculate instead of constructing a new 129 * TrapezoidProfile with the desired and current state 130 */ 131 @Deprecated(since = "2024", forRemoval = true) 132 public TrapezoidProfile(Constraints constraints, State goal, State initial) { 133 m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1; 134 m_constraints = constraints; 135 m_current = direct(initial); 136 m_goal = direct(goal); 137 m_newAPI = false; 138 if (m_current.velocity > m_constraints.maxVelocity) { 139 m_current.velocity = m_constraints.maxVelocity; 140 } 141 142 // Deal with a possibly truncated motion profile (with nonzero initial or 143 // final velocity) by calculating the parameters as if the profile began and 144 // ended at zero velocity 145 double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; 146 double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; 147 148 double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration; 149 double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; 150 151 // Now we can calculate the parameters as if it was a full trapezoid instead 152 // of a truncated one 153 154 double fullTrapezoidDist = 155 cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd; 156 double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; 157 158 double fullSpeedDist = 159 fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; 160 161 // Handle the case where the profile never reaches full speed 162 if (fullSpeedDist < 0) { 163 accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); 164 fullSpeedDist = 0; 165 } 166 167 m_endAccel = accelerationTime - cutoffBegin; 168 m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; 169 m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; 170 } 171 172 /** 173 * Construct a TrapezoidProfile. 174 * 175 * @param constraints The constraints on the profile, like maximum velocity. 176 * @param goal The desired state when the profile is complete. 177 * @deprecated Pass the desired and current state into calculate instead of constructing a new 178 * TrapezoidProfile with the desired and current state 179 */ 180 @Deprecated(since = "2024", forRemoval = true) 181 public TrapezoidProfile(Constraints constraints, State goal) { 182 this(constraints, goal, new State(0, 0)); 183 } 184 185 /** 186 * Calculate the correct position and velocity for the profile at a time t where the beginning of 187 * the profile was at time t = 0. 188 * 189 * @param t The time since the beginning of the profile. 190 * @return The position and velocity of the profile at time t. 191 * @deprecated Pass the desired and current state into calculate instead of constructing a new 192 * TrapezoidProfile with the desired and current state 193 */ 194 @Deprecated(since = "2024", forRemoval = true) 195 public State calculate(double t) { 196 if (m_newAPI) { 197 throw new RuntimeException("Cannot use new constructor with deprecated calculate()"); 198 } 199 State result = new State(m_current.position, m_current.velocity); 200 201 if (t < m_endAccel) { 202 result.velocity += t * m_constraints.maxAcceleration; 203 result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; 204 } else if (t < m_endFullSpeed) { 205 result.velocity = m_constraints.maxVelocity; 206 result.position += 207 (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel 208 + m_constraints.maxVelocity * (t - m_endAccel); 209 } else if (t <= m_endDeccel) { 210 result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; 211 double timeLeft = m_endDeccel - t; 212 result.position = 213 m_goal.position 214 - (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; 215 } else { 216 result = m_goal; 217 } 218 219 return direct(result); 220 } 221 222 /** 223 * Calculate the correct position and velocity for the profile at a time t where the beginning of 224 * the profile was at time t = 0. 225 * 226 * @param t The time since the beginning of the profile. 227 * @param current The current state. 228 * @param goal The desired state when the profile is complete. 229 * @return The position and velocity of the profile at time t. 230 */ 231 public State calculate(double t, State current, State goal) { 232 m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1; 233 m_current = direct(current); 234 goal = direct(goal); 235 236 if (m_current.velocity > m_constraints.maxVelocity) { 237 m_current.velocity = m_constraints.maxVelocity; 238 } 239 240 // Deal with a possibly truncated motion profile (with nonzero initial or 241 // final velocity) by calculating the parameters as if the profile began and 242 // ended at zero velocity 243 double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; 244 double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; 245 246 double cutoffEnd = goal.velocity / m_constraints.maxAcceleration; 247 double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; 248 249 // Now we can calculate the parameters as if it was a full trapezoid instead 250 // of a truncated one 251 252 double fullTrapezoidDist = 253 cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; 254 double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; 255 256 double fullSpeedDist = 257 fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; 258 259 // Handle the case where the profile never reaches full speed 260 if (fullSpeedDist < 0) { 261 accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); 262 fullSpeedDist = 0; 263 } 264 265 m_endAccel = accelerationTime - cutoffBegin; 266 m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; 267 m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; 268 State result = new State(m_current.position, m_current.velocity); 269 270 if (t < m_endAccel) { 271 result.velocity += t * m_constraints.maxAcceleration; 272 result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; 273 } else if (t < m_endFullSpeed) { 274 result.velocity = m_constraints.maxVelocity; 275 result.position += 276 (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel 277 + m_constraints.maxVelocity * (t - m_endAccel); 278 } else if (t <= m_endDeccel) { 279 result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; 280 double timeLeft = m_endDeccel - t; 281 result.position = 282 goal.position 283 - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; 284 } else { 285 result = goal; 286 } 287 288 return direct(result); 289 } 290 291 /** 292 * Returns the time left until a target distance in the profile is reached. 293 * 294 * @param target The target distance. 295 * @return The time left until a target distance in the profile is reached. 296 */ 297 public double timeLeftUntil(double target) { 298 double position = m_current.position * m_direction; 299 double velocity = m_current.velocity * m_direction; 300 301 double endAccel = m_endAccel * m_direction; 302 double endFullSpeed = m_endFullSpeed * m_direction - endAccel; 303 304 if (target < position) { 305 endAccel = -endAccel; 306 endFullSpeed = -endFullSpeed; 307 velocity = -velocity; 308 } 309 310 endAccel = Math.max(endAccel, 0); 311 endFullSpeed = Math.max(endFullSpeed, 0); 312 313 final double acceleration = m_constraints.maxAcceleration; 314 final double decceleration = -m_constraints.maxAcceleration; 315 316 double distToTarget = Math.abs(target - position); 317 if (distToTarget < 1e-6) { 318 return 0; 319 } 320 321 double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel; 322 323 double deccelVelocity; 324 if (endAccel > 0) { 325 deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)); 326 } else { 327 deccelVelocity = velocity; 328 } 329 330 double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed; 331 double deccelDist; 332 333 if (accelDist > distToTarget) { 334 accelDist = distToTarget; 335 fullSpeedDist = 0; 336 deccelDist = 0; 337 } else if (accelDist + fullSpeedDist > distToTarget) { 338 fullSpeedDist = distToTarget - accelDist; 339 deccelDist = 0; 340 } else { 341 deccelDist = distToTarget - fullSpeedDist - accelDist; 342 } 343 344 double accelTime = 345 (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist))) 346 / acceleration; 347 348 double deccelTime = 349 (-deccelVelocity 350 + Math.sqrt( 351 Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist))) 352 / decceleration; 353 354 double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity; 355 356 return accelTime + fullSpeedTime + deccelTime; 357 } 358 359 /** 360 * Returns the total time the profile takes to reach the goal. 361 * 362 * @return The total time the profile takes to reach the goal. 363 */ 364 public double totalTime() { 365 return m_endDeccel; 366 } 367 368 /** 369 * Returns true if the profile has reached the goal. 370 * 371 * <p>The profile has reached the goal if the time since the profile started has exceeded the 372 * profile's total time. 373 * 374 * @param t The time since the beginning of the profile. 375 * @return True if the profile has reached the goal. 376 */ 377 public boolean isFinished(double t) { 378 return t >= totalTime(); 379 } 380 381 /** 382 * Returns true if the profile inverted. 383 * 384 * <p>The profile is inverted if goal position is less than the initial position. 385 * 386 * @param initial The initial state (usually the current state). 387 * @param goal The desired state when the profile is complete. 388 */ 389 private static boolean shouldFlipAcceleration(State initial, State goal) { 390 return initial.position > goal.position; 391 } 392 393 // Flip the sign of the velocity and position if the profile is inverted 394 private State direct(State in) { 395 State result = new State(in.position, in.velocity); 396 result.position = result.position * m_direction; 397 result.velocity = result.velocity * m_direction; 398 return result; 399 } 400}