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 051 private double m_endAccel; 052 private double m_endFullSpeed; 053 private double m_endDeccel; 054 055 /** Profile constraints. */ 056 public static class Constraints { 057 /** Maximum velocity. */ 058 public final double maxVelocity; 059 060 /** Maximum acceleration. */ 061 public final double maxAcceleration; 062 063 /** 064 * Constructs constraints for a TrapezoidProfile. 065 * 066 * @param maxVelocity maximum velocity 067 * @param maxAcceleration maximum acceleration 068 */ 069 public Constraints(double maxVelocity, double maxAcceleration) { 070 this.maxVelocity = maxVelocity; 071 this.maxAcceleration = maxAcceleration; 072 MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1); 073 } 074 075 /** 076 * Constructs constraints for a TrapezoidProfile. 077 * 078 * @param <U> Unit type. 079 * @param maxVelocity maximum velocity 080 * @param maxAcceleration maximum acceleration 081 */ 082 public <U extends Unit<U>> Constraints( 083 Measure<Velocity<U>> maxVelocity, Measure<Velocity<Velocity<U>>> maxAcceleration) { 084 this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude()); 085 } 086 } 087 088 /** Profile state. */ 089 public static class State { 090 /** The position at this state. */ 091 public double position; 092 093 /** The velocity at this state. */ 094 public double velocity; 095 096 /** Default constructor. */ 097 public State() {} 098 099 /** 100 * Constructs constraints for a Trapezoid Profile. 101 * 102 * @param position The position at this state. 103 * @param velocity The velocity at this state. 104 */ 105 public State(double position, double velocity) { 106 this.position = position; 107 this.velocity = velocity; 108 } 109 110 /** 111 * Constructs constraints for a Trapezoid Profile. 112 * 113 * @param <U> Unit type. 114 * @param position The position at this state. 115 * @param velocity The velocity at this state. 116 */ 117 public <U extends Unit<U>> State(Measure<U> position, Measure<Velocity<U>> velocity) { 118 this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude()); 119 } 120 121 @Override 122 public boolean equals(Object other) { 123 if (other instanceof State) { 124 State rhs = (State) other; 125 return this.position == rhs.position && this.velocity == rhs.velocity; 126 } else { 127 return false; 128 } 129 } 130 131 @Override 132 public int hashCode() { 133 return Objects.hash(position, velocity); 134 } 135 } 136 137 /** 138 * Constructs a TrapezoidProfile. 139 * 140 * @param constraints The constraints on the profile, like maximum velocity. 141 */ 142 public TrapezoidProfile(Constraints constraints) { 143 m_constraints = constraints; 144 } 145 146 /** 147 * Calculates the position and velocity for the profile at a time t where the current state is at 148 * time t = 0. 149 * 150 * @param t How long to advance from the current state toward the desired state. 151 * @param current The current state. 152 * @param goal The desired state when the profile is complete. 153 * @return The position and velocity of the profile at time t. 154 */ 155 public State calculate(double t, State current, State goal) { 156 m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1; 157 m_current = direct(current); 158 goal = direct(goal); 159 160 if (m_current.velocity > m_constraints.maxVelocity) { 161 m_current.velocity = m_constraints.maxVelocity; 162 } 163 164 // Deal with a possibly truncated motion profile (with nonzero initial or 165 // final velocity) by calculating the parameters as if the profile began and 166 // ended at zero velocity 167 double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; 168 double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; 169 170 double cutoffEnd = goal.velocity / m_constraints.maxAcceleration; 171 double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; 172 173 // Now we can calculate the parameters as if it was a full trapezoid instead 174 // of a truncated one 175 176 double fullTrapezoidDist = 177 cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; 178 double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; 179 180 double fullSpeedDist = 181 fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; 182 183 // Handle the case where the profile never reaches full speed 184 if (fullSpeedDist < 0) { 185 accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); 186 fullSpeedDist = 0; 187 } 188 189 m_endAccel = accelerationTime - cutoffBegin; 190 m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; 191 m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; 192 State result = new State(m_current.position, m_current.velocity); 193 194 if (t < m_endAccel) { 195 result.velocity += t * m_constraints.maxAcceleration; 196 result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; 197 } else if (t < m_endFullSpeed) { 198 result.velocity = m_constraints.maxVelocity; 199 result.position += 200 (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel 201 + m_constraints.maxVelocity * (t - m_endAccel); 202 } else if (t <= m_endDeccel) { 203 result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; 204 double timeLeft = m_endDeccel - t; 205 result.position = 206 goal.position 207 - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; 208 } else { 209 result = goal; 210 } 211 212 return direct(result); 213 } 214 215 /** 216 * Returns the time left until a target distance in the profile is reached. 217 * 218 * @param target The target distance. 219 * @return The time left until a target distance in the profile is reached. 220 */ 221 public double timeLeftUntil(double target) { 222 double position = m_current.position * m_direction; 223 double velocity = m_current.velocity * m_direction; 224 225 double endAccel = m_endAccel * m_direction; 226 double endFullSpeed = m_endFullSpeed * m_direction - endAccel; 227 228 if (target < position) { 229 endAccel = -endAccel; 230 endFullSpeed = -endFullSpeed; 231 velocity = -velocity; 232 } 233 234 endAccel = Math.max(endAccel, 0); 235 endFullSpeed = Math.max(endFullSpeed, 0); 236 237 final double acceleration = m_constraints.maxAcceleration; 238 final double decceleration = -m_constraints.maxAcceleration; 239 240 double distToTarget = Math.abs(target - position); 241 if (distToTarget < 1e-6) { 242 return 0; 243 } 244 245 double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel; 246 247 double deccelVelocity; 248 if (endAccel > 0) { 249 deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)); 250 } else { 251 deccelVelocity = velocity; 252 } 253 254 double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed; 255 double deccelDist; 256 257 if (accelDist > distToTarget) { 258 accelDist = distToTarget; 259 fullSpeedDist = 0; 260 deccelDist = 0; 261 } else if (accelDist + fullSpeedDist > distToTarget) { 262 fullSpeedDist = distToTarget - accelDist; 263 deccelDist = 0; 264 } else { 265 deccelDist = distToTarget - fullSpeedDist - accelDist; 266 } 267 268 double accelTime = 269 (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist))) 270 / acceleration; 271 272 double deccelTime = 273 (-deccelVelocity 274 + Math.sqrt( 275 Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist))) 276 / decceleration; 277 278 double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity; 279 280 return accelTime + fullSpeedTime + deccelTime; 281 } 282 283 /** 284 * Returns the total time the profile takes to reach the goal. 285 * 286 * @return The total time the profile takes to reach the goal. 287 */ 288 public double totalTime() { 289 return m_endDeccel; 290 } 291 292 /** 293 * Returns true if the profile has reached the goal. 294 * 295 * <p>The profile has reached the goal if the time since the profile started has exceeded the 296 * profile's total time. 297 * 298 * @param t The time since the beginning of the profile. 299 * @return True if the profile has reached the goal. 300 */ 301 public boolean isFinished(double t) { 302 return t >= totalTime(); 303 } 304 305 /** 306 * Returns true if the profile inverted. 307 * 308 * <p>The profile is inverted if goal position is less than the initial position. 309 * 310 * @param initial The initial state (usually the current state). 311 * @param goal The desired state when the profile is complete. 312 */ 313 private static boolean shouldFlipAcceleration(State initial, State goal) { 314 return initial.position > goal.position; 315 } 316 317 // Flip the sign of the velocity and position if the profile is inverted 318 private State direct(State in) { 319 State result = new State(in.position, in.velocity); 320 result.position = result.position * m_direction; 321 result.velocity = result.velocity * m_direction; 322 return result; 323 } 324}