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 java.util.Objects; 008 009/** 010 * A exponential curve-shaped velocity profile. 011 * 012 * <p>While this class can be used for a profiled movement from start to finish, the intended usage 013 * is to filter a reference's dynamics based on state-space model dynamics. To compute the reference 014 * obeying this constraint, do the following. 015 * 016 * <p>Initialization: 017 * 018 * <pre><code> 019 * ExponentialProfile.Constraints constraints = 020 * ExponentialProfile.Constraints.fromCharacteristics(kMaxV, kV, kA); 021 * ExponentialProfile.State previousProfiledReference = 022 * new ExponentialProfile.State(initialReference, 0.0); 023 * ExponentialProfile profile = new ExponentialProfile(constraints); 024 * </code></pre> 025 * 026 * <p>Run on update: 027 * 028 * <pre><code> 029 * previousProfiledReference = 030 * profile.calculate(timeSincePreviousUpdate, previousProfiledReference, unprofiledReference); 031 * </code></pre> 032 * 033 * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled 034 * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged. 035 * 036 * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to 037 * determine when the profile has completed via `isFinished()`. 038 */ 039public class ExponentialProfile { 040 private final Constraints m_constraints; 041 042 public static class ProfileTiming { 043 public final double inflectionTime; 044 public final double totalTime; 045 046 protected ProfileTiming(double inflectionTime, double totalTime) { 047 this.inflectionTime = inflectionTime; 048 this.totalTime = totalTime; 049 } 050 051 /** 052 * Decides if the profile is finished by time t. 053 * 054 * @param t The time since the beginning of the profile. 055 * @return if the profile is finished at time t. 056 */ 057 public boolean isFinished(double t) { 058 return t > inflectionTime; 059 } 060 } 061 062 public static class Constraints { 063 public final double maxInput; 064 065 public final double A; 066 public final double B; 067 068 /** 069 * Construct constraints for an ExponentialProfile. 070 * 071 * @param maxInput maximum unsigned input voltage 072 * @param A The State-Space 1x1 system matrix. 073 * @param B The State-Space 1x1 input matrix. 074 */ 075 private Constraints(double maxInput, double A, double B) { 076 this.maxInput = maxInput; 077 this.A = A; 078 this.B = B; 079 } 080 081 /** 082 * Computes the max achievable velocity for an Exponential Profile. 083 * 084 * @return The seady-state velocity achieved by this profile. 085 */ 086 public double maxVelocity() { 087 return -maxInput * B / A; 088 } 089 090 /** 091 * Construct constraints for an ExponentialProfile from characteristics. 092 * 093 * @param maxInput maximum unsigned input voltage 094 * @param kV The velocity gain. 095 * @param kA The acceleration gain. 096 * @return The Constraints object. 097 */ 098 public static Constraints fromCharacteristics(double maxInput, double kV, double kA) { 099 return new Constraints(maxInput, -kV / kA, 1.0 / kA); 100 } 101 102 /** 103 * Construct constraints for an ExponentialProfile from State-Space parameters. 104 * 105 * @param maxInput maximum unsigned input voltage 106 * @param A The State-Space 1x1 system matrix. 107 * @param B The State-Space 1x1 input matrix. 108 * @return The Constraints object. 109 */ 110 public static Constraints fromStateSpace(double maxInput, double A, double B) { 111 return new Constraints(maxInput, A, B); 112 } 113 } 114 115 public static class State { 116 public final double position; 117 118 public final double velocity; 119 120 /** 121 * Construct a state within an exponential profile. 122 * 123 * @param position The position at this state. 124 * @param velocity The velocity at this state. 125 */ 126 public State(double position, double velocity) { 127 this.position = position; 128 this.velocity = velocity; 129 } 130 131 @Override 132 public boolean equals(Object other) { 133 if (other instanceof State) { 134 State rhs = (State) other; 135 return this.position == rhs.position && this.velocity == rhs.velocity; 136 } else { 137 return false; 138 } 139 } 140 141 @Override 142 public int hashCode() { 143 return Objects.hash(position, velocity); 144 } 145 } 146 147 /** 148 * Construct an ExponentialProfile. 149 * 150 * @param constraints The constraints on the profile, like maximum input. 151 */ 152 public ExponentialProfile(Constraints constraints) { 153 m_constraints = constraints; 154 } 155 156 /** 157 * Calculate the correct position and velocity for the profile at a time t where the current state 158 * is at time t = 0. 159 * 160 * @param t The time since the beginning of the profile. 161 * @param current The current state. 162 * @param goal The desired state when the profile is complete. 163 * @return The position and velocity of the profile at time t. 164 */ 165 public State calculate(double t, State current, State goal) { 166 var direction = shouldFlipInput(current, goal) ? -1 : 1; 167 var u = direction * m_constraints.maxInput; 168 169 var inflectionPoint = calculateInflectionPoint(current, goal, u); 170 var timing = calculateProfileTiming(current, inflectionPoint, goal, u); 171 172 if (t < 0) { 173 return current; 174 } else if (t < timing.inflectionTime) { 175 return new State( 176 computeDistanceFromTime(t, u, current), computeVelocityFromTime(t, u, current)); 177 } else if (t < timing.totalTime) { 178 return new State( 179 computeDistanceFromTime(t - timing.totalTime, -u, goal), 180 computeVelocityFromTime(t - timing.totalTime, -u, goal)); 181 } else { 182 return goal; 183 } 184 } 185 186 /** 187 * Calculate the point after which the fastest way to reach the goal state is to apply input in 188 * the opposite direction. 189 * 190 * @param current The current state. 191 * @param goal The desired state when the profile is complete. 192 * @return The position and velocity of the profile at the inflection point. 193 */ 194 public State calculateInflectionPoint(State current, State goal) { 195 var direction = shouldFlipInput(current, goal) ? -1 : 1; 196 var u = direction * m_constraints.maxInput; 197 198 return calculateInflectionPoint(current, goal, u); 199 } 200 201 /** 202 * Calculate the point after which the fastest way to reach the goal state is to apply input in 203 * the opposite direction. 204 * 205 * @param current The current state. 206 * @param goal The desired state when the profile is complete. 207 * @param input The signed input applied to this profile from the current state. 208 * @return The position and velocity of the profile at the inflection point. 209 */ 210 private State calculateInflectionPoint(State current, State goal, double input) { 211 var u = input; 212 213 if (current.equals(goal)) { 214 return current; 215 } 216 217 var inflectionVelocity = solveForInflectionVelocity(u, current, goal); 218 var inflectionPosition = computeDistanceFromVelocity(inflectionVelocity, -u, goal); 219 220 return new State(inflectionPosition, inflectionVelocity); 221 } 222 223 /** 224 * Calculate the time it will take for this profile to reach the goal state. 225 * 226 * @param current The current state. 227 * @param goal The desired state when the profile is complete. 228 * @return The total duration of this profile. 229 */ 230 public double timeLeftUntil(State current, State goal) { 231 var timing = calculateProfileTiming(current, goal); 232 233 return timing.totalTime; 234 } 235 236 /** 237 * Calculate the time it will take for this profile to reach the inflection point, and the time it 238 * will take for this profile to reach the goal state. 239 * 240 * @param current The current state. 241 * @param goal The desired state when the profile is complete. 242 * @return The timing information for this profile. 243 */ 244 public ProfileTiming calculateProfileTiming(State current, State goal) { 245 var direction = shouldFlipInput(current, goal) ? -1 : 1; 246 var u = direction * m_constraints.maxInput; 247 248 var inflectionPoint = calculateInflectionPoint(current, goal, u); 249 return calculateProfileTiming(current, inflectionPoint, goal, u); 250 } 251 252 /** 253 * Calculate the time it will take for this profile to reach the inflection point, and the time it 254 * will take for this profile to reach the goal state. 255 * 256 * @param current The current state. 257 * @param inflectionPoint The inflection point of this profile. 258 * @param goal The desired state when the profile is complete. 259 * @param input The signed input applied to this profile from the current state. 260 * @return The timing information for this profile. 261 */ 262 private ProfileTiming calculateProfileTiming( 263 State current, State inflectionPoint, State goal, double input) { 264 var u = input; 265 266 double inflectionT_forward; 267 268 // We need to handle 5 cases here: 269 // 270 // - Approaching -maxVelocity from below 271 // - Approaching -maxVelocity from above 272 // - Approaching maxVelocity from below 273 // - Approaching maxVelocity from above 274 // - At +-maxVelocity 275 // 276 // For cases 1 and 3, we want to subtract epsilon from the inflection point velocity. 277 // For cases 2 and 4, we want to add epsilon to the inflection point velocity. 278 // For case 5, we have reached inflection point velocity. 279 double epsilon = 1e-9; 280 if (Math.abs(Math.signum(input) * m_constraints.maxVelocity() - inflectionPoint.velocity) 281 < epsilon) { 282 double solvableV = inflectionPoint.velocity; 283 double t_to_solvable_v; 284 double x_at_solvable_v; 285 if (Math.abs(current.velocity - inflectionPoint.velocity) < epsilon) { 286 t_to_solvable_v = 0; 287 x_at_solvable_v = current.position; 288 } else { 289 if (Math.abs(current.velocity) > m_constraints.maxVelocity()) { 290 solvableV += Math.signum(u) * epsilon; 291 } else { 292 solvableV -= Math.signum(u) * epsilon; 293 } 294 295 t_to_solvable_v = computeTimeFromVelocity(solvableV, u, current.velocity); 296 x_at_solvable_v = computeDistanceFromVelocity(solvableV, u, current); 297 } 298 299 inflectionT_forward = 300 t_to_solvable_v 301 + Math.signum(input) 302 * (inflectionPoint.position - x_at_solvable_v) 303 / m_constraints.maxVelocity(); 304 } else { 305 inflectionT_forward = computeTimeFromVelocity(inflectionPoint.velocity, u, current.velocity); 306 } 307 308 var inflectionT_backward = computeTimeFromVelocity(inflectionPoint.velocity, -u, goal.velocity); 309 310 return new ProfileTiming(inflectionT_forward, inflectionT_forward - inflectionT_backward); 311 } 312 313 /** 314 * Calculate the position reached after t seconds when applying an input from the initial state. 315 * 316 * @param t The time since the initial state. 317 * @param input The signed input applied to this profile from the initial state. 318 * @param initial The initial state. 319 * @return The distance travelled by this profile. 320 */ 321 private double computeDistanceFromTime(double t, double input, State initial) { 322 var A = m_constraints.A; 323 var B = m_constraints.B; 324 var u = input; 325 326 return initial.position 327 + (-B * u * t + (initial.velocity + B * u / A) * (Math.exp(A * t) - 1)) / A; 328 } 329 330 /** 331 * Calculate the velocity reached after t seconds when applying an input from the initial state. 332 * 333 * @param t The time since the initial state. 334 * @param input The signed input applied to this profile from the initial state. 335 * @param initial The initial state. 336 * @return The distance travelled by this profile. 337 */ 338 private double computeVelocityFromTime(double t, double input, State initial) { 339 var A = m_constraints.A; 340 var B = m_constraints.B; 341 var u = input; 342 343 return (initial.velocity + B * u / A) * Math.exp(A * t) - B * u / A; 344 } 345 346 /** 347 * Calculate the time required to reach a specified velocity given the initial velocity. 348 * 349 * @param velocity The goal velocity. 350 * @param input The signed input applied to this profile from the initial state. 351 * @param initial The initial velocity. 352 * @return The time required to reach the goal velocity. 353 */ 354 private double computeTimeFromVelocity(double velocity, double input, double initial) { 355 var A = m_constraints.A; 356 var B = m_constraints.B; 357 var u = input; 358 359 return Math.log((A * velocity + B * u) / (A * initial + B * u)) / A; 360 } 361 362 /** 363 * Calculate the distance reached at the same time as the given velocity when applying the given 364 * input from the initial state. 365 * 366 * @param velocity The velocity reached by this profile 367 * @param input The signed input applied to this profile from the initial state. 368 * @param initial The initial state. 369 * @return The distance reached when the given velocity is reached. 370 */ 371 private double computeDistanceFromVelocity(double velocity, double input, State initial) { 372 var A = m_constraints.A; 373 var B = m_constraints.B; 374 var u = input; 375 376 return initial.position 377 + (velocity - initial.velocity) / A 378 - B * u / (A * A) * Math.log((A * velocity + B * u) / (A * initial.velocity + B * u)); 379 } 380 381 /** 382 * Calculate the velocity at which input should be reversed in order to reach the goal state from 383 * the current state. 384 * 385 * @param input The signed input applied to this profile from the current state. 386 * @param current The current state. 387 * @param goal The goal state. 388 * @return The inflection velocity. 389 */ 390 private double solveForInflectionVelocity(double input, State current, State goal) { 391 var A = m_constraints.A; 392 var B = m_constraints.B; 393 var u = input; 394 395 var U_dir = Math.signum(u); 396 397 var position_delta = goal.position - current.position; 398 var velocity_delta = goal.velocity - current.velocity; 399 400 var scalar = (A * current.velocity + B * u) * (A * goal.velocity - B * u); 401 var power = -A / B / u * (A * position_delta - velocity_delta); 402 403 var a = -A * A; 404 var c = (B * B) * (u * u) + scalar * Math.exp(power); 405 406 if (-1e-9 < c && c < 0) { 407 // Numerical stability issue - the heuristic gets it right but c is around -1e-13 408 return 0; 409 } 410 411 return U_dir * Math.sqrt(-c / a); 412 } 413 414 /** 415 * Returns true if the profile should be inverted. 416 * 417 * <p>The profile is inverted if we should first apply negative input in order to reach the goal 418 * state. 419 * 420 * @param current The initial state (usually the current state). 421 * @param goal The desired state when the profile is complete. 422 */ 423 @SuppressWarnings("UnnecessaryParentheses") 424 private boolean shouldFlipInput(State current, State goal) { 425 var u = m_constraints.maxInput; 426 427 var xf = goal.position; 428 var v0 = current.velocity; 429 var vf = goal.velocity; 430 431 var x_forward = computeDistanceFromVelocity(vf, u, current); 432 var x_reverse = computeDistanceFromVelocity(vf, -u, current); 433 434 if (v0 >= m_constraints.maxVelocity()) { 435 return xf < x_reverse; 436 } 437 438 if (v0 <= -m_constraints.maxVelocity()) { 439 return xf < x_forward; 440 } 441 442 var a = v0 >= 0; 443 var b = vf >= 0; 444 var c = xf >= x_forward; 445 var d = xf >= x_reverse; 446 447 return (a && !d) || (b && !c) || (!c && !d); 448 } 449}