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 java.util.Objects; 009 010/** 011 * A trapezoid-shaped velocity profile. 012 * 013 * <p>While this class can be used for a profiled movement from start to finish, the intended usage 014 * is to filter a reference's dynamics based on trapezoidal velocity constraints. To compute the 015 * reference obeying this constraint, do the following. 016 * 017 * <p>Initialization: 018 * 019 * <pre><code> 020 * TrapezoidProfile.Constraints constraints = 021 * new TrapezoidProfile.Constraints(kMaxV, kMaxA); 022 * TrapezoidProfile.State previousProfiledReference = 023 * new TrapezoidProfile.State(initialReference, 0.0); 024 * TrapezoidProfile profile = new TrapezoidProfile(constraints); 025 * </code></pre> 026 * 027 * <p>Run on update: 028 * 029 * <pre><code> 030 * previousProfiledReference = 031 * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference); 032 * </code></pre> 033 * 034 * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled 035 * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged. 036 * 037 * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to 038 * determine when the profile has completed via `isFinished()`. 039 */ 040public class TrapezoidProfile { 041 // The direction of the profile, either 1 for forwards or -1 for inverted 042 private int m_direction; 043 044 private final Constraints m_constraints; 045 private State m_current = new State(); 046 047 private double m_endAccel; 048 private double m_endFullSpeed; 049 private double m_endDecel; 050 051 /** Profile constraints. */ 052 public static class Constraints { 053 /** Maximum velocity. */ 054 public final double maxVelocity; 055 056 /** Maximum acceleration. */ 057 public final double maxAcceleration; 058 059 /** 060 * Constructs constraints for a TrapezoidProfile. 061 * 062 * @param maxVelocity Maximum velocity, must be non-negative. 063 * @param maxAcceleration Maximum acceleration, must be non-negative. 064 */ 065 public Constraints(double maxVelocity, double maxAcceleration) { 066 if (maxVelocity < 0.0 || maxAcceleration < 0.0) { 067 throw new IllegalArgumentException("Constraints must be non-negative"); 068 } 069 070 this.maxVelocity = maxVelocity; 071 this.maxAcceleration = maxAcceleration; 072 MathSharedStore.reportUsage("TrapezoidProfile", ""); 073 } 074 } 075 076 /** Profile state. */ 077 public static class State { 078 /** The position at this state. */ 079 public double position; 080 081 /** The velocity at this state. */ 082 public double velocity; 083 084 /** Default constructor. */ 085 public State() {} 086 087 /** 088 * Constructs constraints for a Trapezoid Profile. 089 * 090 * @param position The position at this state. 091 * @param velocity The velocity at this state. 092 */ 093 public State(double position, double velocity) { 094 this.position = position; 095 this.velocity = velocity; 096 } 097 098 @Override 099 public boolean equals(Object other) { 100 return other instanceof State rhs 101 && this.position == rhs.position 102 && this.velocity == rhs.velocity; 103 } 104 105 @Override 106 public int hashCode() { 107 return Objects.hash(position, velocity); 108 } 109 } 110 111 /** 112 * Constructs a TrapezoidProfile. 113 * 114 * @param constraints The constraints on the profile, like maximum velocity. 115 */ 116 public TrapezoidProfile(Constraints constraints) { 117 m_constraints = constraints; 118 } 119 120 /** 121 * Calculates the position and velocity for the profile at a time t where the current state is at 122 * time t = 0. 123 * 124 * @param t How long to advance from the current state toward the desired state. 125 * @param current The current state. 126 * @param goal The desired state when the profile is complete. 127 * @return The position and velocity of the profile at time t. 128 */ 129 public State calculate(double t, State current, State goal) { 130 m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1; 131 m_current = direct(current); 132 goal = direct(goal); 133 134 if (Math.abs(m_current.velocity) > m_constraints.maxVelocity) { 135 m_current.velocity = Math.copySign(m_constraints.maxVelocity, m_current.velocity); 136 } 137 138 // Deal with a possibly truncated motion profile (with nonzero initial or 139 // final velocity) by calculating the parameters as if the profile began and 140 // ended at zero velocity 141 double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; 142 double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; 143 144 double cutoffEnd = goal.velocity / m_constraints.maxAcceleration; 145 double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; 146 147 // Now we can calculate the parameters as if it was a full trapezoid instead 148 // of a truncated one 149 150 double fullTrapezoidDist = 151 cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; 152 double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; 153 154 double fullSpeedDist = 155 fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; 156 157 // Handle the case where the profile never reaches full speed 158 if (fullSpeedDist < 0) { 159 accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); 160 fullSpeedDist = 0; 161 } 162 163 m_endAccel = accelerationTime - cutoffBegin; 164 m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; 165 m_endDecel = m_endFullSpeed + accelerationTime - cutoffEnd; 166 State result = new State(m_current.position, m_current.velocity); 167 168 if (t < m_endAccel) { 169 result.velocity += t * m_constraints.maxAcceleration; 170 result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; 171 } else if (t < m_endFullSpeed) { 172 result.velocity = m_constraints.maxVelocity; 173 result.position += 174 (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel 175 + m_constraints.maxVelocity * (t - m_endAccel); 176 } else if (t <= m_endDecel) { 177 result.velocity = goal.velocity + (m_endDecel - t) * m_constraints.maxAcceleration; 178 double timeLeft = m_endDecel - t; 179 result.position = 180 goal.position 181 - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; 182 } else { 183 result = goal; 184 } 185 186 return direct(result); 187 } 188 189 /** 190 * Returns the time left until a target distance in the profile is reached. 191 * 192 * @param target The target distance. 193 * @return The time left until a target distance in the profile is reached, or zero if no goal was 194 * set. 195 */ 196 public double timeLeftUntil(double target) { 197 double position = m_current.position * m_direction; 198 double velocity = m_current.velocity * m_direction; 199 200 double endAccel = m_endAccel * m_direction; 201 double endFullSpeed = m_endFullSpeed * m_direction - endAccel; 202 203 if (target < position) { 204 endAccel = -endAccel; 205 endFullSpeed = -endFullSpeed; 206 velocity = -velocity; 207 } 208 209 endAccel = Math.max(endAccel, 0); 210 endFullSpeed = Math.max(endFullSpeed, 0); 211 212 final double acceleration = m_constraints.maxAcceleration; 213 final double deceleration = -m_constraints.maxAcceleration; 214 215 double distToTarget = Math.abs(target - position); 216 if (distToTarget < 1e-6) { 217 return 0; 218 } 219 220 double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel; 221 222 double decelVelocity; 223 if (endAccel > 0) { 224 decelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)); 225 } else { 226 decelVelocity = velocity; 227 } 228 229 double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed; 230 double decelDist; 231 232 if (accelDist > distToTarget) { 233 accelDist = distToTarget; 234 fullSpeedDist = 0; 235 decelDist = 0; 236 } else if (accelDist + fullSpeedDist > distToTarget) { 237 fullSpeedDist = distToTarget - accelDist; 238 decelDist = 0; 239 } else { 240 decelDist = distToTarget - fullSpeedDist - accelDist; 241 } 242 243 double accelTime = 244 (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist))) 245 / acceleration; 246 247 double decelTime = 248 (-decelVelocity 249 + Math.sqrt(Math.abs(decelVelocity * decelVelocity + 2 * deceleration * decelDist))) 250 / deceleration; 251 252 double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity; 253 254 return accelTime + fullSpeedTime + decelTime; 255 } 256 257 /** 258 * Returns the total time the profile takes to reach the goal. 259 * 260 * @return The total time the profile takes to reach the goal, or zero if no goal was set. 261 */ 262 public double totalTime() { 263 return m_endDecel; 264 } 265 266 /** 267 * Returns true if the profile has reached the goal. 268 * 269 * <p>The profile has reached the goal if the time since the profile started has exceeded the 270 * profile's total time. 271 * 272 * @param t The time since the beginning of the profile. 273 * @return True if the profile has reached the goal. 274 */ 275 public boolean isFinished(double t) { 276 return t >= totalTime(); 277 } 278 279 /** 280 * Returns true if the profile inverted. 281 * 282 * <p>The profile is inverted if goal position is less than the initial position. 283 * 284 * @param initial The initial state (usually the current state). 285 * @param goal The desired state when the profile is complete. 286 */ 287 private static boolean shouldFlipAcceleration(State initial, State goal) { 288 return initial.position > goal.position; 289 } 290 291 // Flip the sign of the velocity and position if the profile is inverted 292 private State direct(State in) { 293 State result = new State(in.position, in.velocity); 294 result.position = result.position * m_direction; 295 result.velocity = result.velocity * m_direction; 296 return result; 297 } 298}