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 com.fasterxml.jackson.annotation.JsonProperty; 008import edu.wpi.first.math.geometry.Pose2d; 009import edu.wpi.first.math.geometry.Transform2d; 010import edu.wpi.first.math.trajectory.proto.TrajectoryProto; 011import edu.wpi.first.math.trajectory.proto.TrajectoryStateProto; 012import java.util.ArrayList; 013import java.util.List; 014import java.util.Objects; 015import java.util.stream.Collectors; 016 017/** 018 * Represents a time-parameterized trajectory. The trajectory contains of various States that 019 * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. 020 */ 021public class Trajectory { 022 public static final TrajectoryProto proto = new TrajectoryProto(); 023 024 private final double m_totalTimeSeconds; 025 private final List<State> m_states; 026 027 /** Constructs an empty trajectory. */ 028 public Trajectory() { 029 m_states = new ArrayList<>(); 030 m_totalTimeSeconds = 0.0; 031 } 032 033 /** 034 * Constructs a trajectory from a vector of states. 035 * 036 * @param states A vector of states. 037 * @throws IllegalArgumentException if the vector of states is empty. 038 */ 039 public Trajectory(final List<State> states) { 040 m_states = states; 041 042 if (m_states.isEmpty()) { 043 throw new IllegalArgumentException("Trajectory manually created with no states."); 044 } 045 046 m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds; 047 } 048 049 /** 050 * Linearly interpolates between two values. 051 * 052 * @param startValue The start value. 053 * @param endValue The end value. 054 * @param t The fraction for interpolation. 055 * @return The interpolated value. 056 */ 057 private static double lerp(double startValue, double endValue, double t) { 058 return startValue + (endValue - startValue) * t; 059 } 060 061 /** 062 * Linearly interpolates between two poses. 063 * 064 * @param startValue The start pose. 065 * @param endValue The end pose. 066 * @param t The fraction for interpolation. 067 * @return The interpolated pose. 068 */ 069 private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) { 070 return startValue.plus((endValue.minus(startValue)).times(t)); 071 } 072 073 /** 074 * Returns the initial pose of the trajectory. 075 * 076 * @return The initial pose of the trajectory. 077 */ 078 public Pose2d getInitialPose() { 079 return sample(0).poseMeters; 080 } 081 082 /** 083 * Returns the overall duration of the trajectory. 084 * 085 * @return The duration of the trajectory. 086 */ 087 public double getTotalTimeSeconds() { 088 return m_totalTimeSeconds; 089 } 090 091 /** 092 * Return the states of the trajectory. 093 * 094 * @return The states of the trajectory. 095 */ 096 public List<State> getStates() { 097 return m_states; 098 } 099 100 /** 101 * Sample the trajectory at a point in time. 102 * 103 * @param timeSeconds The point in time since the beginning of the trajectory to sample. 104 * @return The state at that point in time. 105 * @throws IllegalStateException if the trajectory has no states. 106 */ 107 public State sample(double timeSeconds) { 108 if (m_states.isEmpty()) { 109 throw new IllegalStateException("Trajectory cannot be sampled if it has no states."); 110 } 111 112 if (timeSeconds <= m_states.get(0).timeSeconds) { 113 return m_states.get(0); 114 } 115 if (timeSeconds >= m_totalTimeSeconds) { 116 return m_states.get(m_states.size() - 1); 117 } 118 119 // To get the element that we want, we will use a binary search algorithm 120 // instead of iterating over a for-loop. A binary search is O(std::log(n)) 121 // whereas searching using a loop is O(n). 122 123 // This starts at 1 because we use the previous state later on for 124 // interpolation. 125 int low = 1; 126 int high = m_states.size() - 1; 127 128 while (low != high) { 129 int mid = (low + high) / 2; 130 if (m_states.get(mid).timeSeconds < timeSeconds) { 131 // This index and everything under it are less than the requested 132 // timestamp. Therefore, we can discard them. 133 low = mid + 1; 134 } else { 135 // t is at least as large as the element at this index. This means that 136 // anything after it cannot be what we are looking for. 137 high = mid; 138 } 139 } 140 141 // High and Low should be the same. 142 143 // The sample's timestamp is now greater than or equal to the requested 144 // timestamp. If it is greater, we need to interpolate between the 145 // previous state and the current state to get the exact state that we 146 // want. 147 final State sample = m_states.get(low); 148 final State prevSample = m_states.get(low - 1); 149 150 // If the difference in states is negligible, then we are spot on! 151 if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) { 152 return sample; 153 } 154 // Interpolate between the two states for the state that we want. 155 return prevSample.interpolate( 156 sample, 157 (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds)); 158 } 159 160 /** 161 * Transforms all poses in the trajectory by the given transform. This is useful for converting a 162 * robot-relative trajectory into a field-relative trajectory. This works with respect to the 163 * first pose in the trajectory. 164 * 165 * @param transform The transform to transform the trajectory by. 166 * @return The transformed trajectory. 167 */ 168 public Trajectory transformBy(Transform2d transform) { 169 var firstState = m_states.get(0); 170 var firstPose = firstState.poseMeters; 171 172 // Calculate the transformed first pose. 173 var newFirstPose = firstPose.plus(transform); 174 List<State> newStates = new ArrayList<>(); 175 176 newStates.add( 177 new State( 178 firstState.timeSeconds, 179 firstState.velocityMetersPerSecond, 180 firstState.accelerationMetersPerSecondSq, 181 newFirstPose, 182 firstState.curvatureRadPerMeter)); 183 184 for (int i = 1; i < m_states.size(); i++) { 185 var state = m_states.get(i); 186 // We are transforming relative to the coordinate frame of the new initial pose. 187 newStates.add( 188 new State( 189 state.timeSeconds, 190 state.velocityMetersPerSecond, 191 state.accelerationMetersPerSecondSq, 192 newFirstPose.plus(state.poseMeters.minus(firstPose)), 193 state.curvatureRadPerMeter)); 194 } 195 196 return new Trajectory(newStates); 197 } 198 199 /** 200 * Transforms all poses in the trajectory so that they are relative to the given pose. This is 201 * useful for converting a field-relative trajectory into a robot-relative trajectory. 202 * 203 * @param pose The pose that is the origin of the coordinate frame that the current trajectory 204 * will be transformed into. 205 * @return The transformed trajectory. 206 */ 207 public Trajectory relativeTo(Pose2d pose) { 208 return new Trajectory( 209 m_states.stream() 210 .map( 211 state -> 212 new State( 213 state.timeSeconds, 214 state.velocityMetersPerSecond, 215 state.accelerationMetersPerSecondSq, 216 state.poseMeters.relativeTo(pose), 217 state.curvatureRadPerMeter)) 218 .collect(Collectors.toList())); 219 } 220 221 /** 222 * Concatenates another trajectory to the current trajectory. The user is responsible for making 223 * sure that the end pose of this trajectory and the start pose of the other trajectory match (if 224 * that is the desired behavior). 225 * 226 * @param other The trajectory to concatenate. 227 * @return The concatenated trajectory. 228 */ 229 public Trajectory concatenate(Trajectory other) { 230 // If this is a default constructed trajectory with no states, then we can 231 // simply return the rhs trajectory. 232 if (m_states.isEmpty()) { 233 return other; 234 } 235 236 // Deep copy the current states. 237 List<State> states = 238 m_states.stream() 239 .map( 240 state -> 241 new State( 242 state.timeSeconds, 243 state.velocityMetersPerSecond, 244 state.accelerationMetersPerSecondSq, 245 state.poseMeters, 246 state.curvatureRadPerMeter)) 247 .collect(Collectors.toList()); 248 249 // Here we omit the first state of the other trajectory because we don't want 250 // two time points with different states. Sample() will automatically 251 // interpolate between the end of this trajectory and the second state of the 252 // other trajectory. 253 for (int i = 1; i < other.getStates().size(); ++i) { 254 var s = other.getStates().get(i); 255 states.add( 256 new State( 257 s.timeSeconds + m_totalTimeSeconds, 258 s.velocityMetersPerSecond, 259 s.accelerationMetersPerSecondSq, 260 s.poseMeters, 261 s.curvatureRadPerMeter)); 262 } 263 return new Trajectory(states); 264 } 265 266 /** 267 * Represents a time-parameterized trajectory. The trajectory contains of various States that 268 * represent the pose, curvature, time elapsed, velocity, and acceleration at that point. 269 */ 270 public static class State { 271 public static final TrajectoryStateProto proto = new TrajectoryStateProto(); 272 273 // The time elapsed since the beginning of the trajectory. 274 @JsonProperty("time") 275 public double timeSeconds; 276 277 // The speed at that point of the trajectory. 278 @JsonProperty("velocity") 279 public double velocityMetersPerSecond; 280 281 // The acceleration at that point of the trajectory. 282 @JsonProperty("acceleration") 283 public double accelerationMetersPerSecondSq; 284 285 // The pose at that point of the trajectory. 286 @JsonProperty("pose") 287 public Pose2d poseMeters; 288 289 // The curvature at that point of the trajectory. 290 @JsonProperty("curvature") 291 public double curvatureRadPerMeter; 292 293 public State() { 294 poseMeters = new Pose2d(); 295 } 296 297 /** 298 * Constructs a State with the specified parameters. 299 * 300 * @param timeSeconds The time elapsed since the beginning of the trajectory. 301 * @param velocityMetersPerSecond The speed at that point of the trajectory. 302 * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory. 303 * @param poseMeters The pose at that point of the trajectory. 304 * @param curvatureRadPerMeter The curvature at that point of the trajectory. 305 */ 306 public State( 307 double timeSeconds, 308 double velocityMetersPerSecond, 309 double accelerationMetersPerSecondSq, 310 Pose2d poseMeters, 311 double curvatureRadPerMeter) { 312 this.timeSeconds = timeSeconds; 313 this.velocityMetersPerSecond = velocityMetersPerSecond; 314 this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq; 315 this.poseMeters = poseMeters; 316 this.curvatureRadPerMeter = curvatureRadPerMeter; 317 } 318 319 /** 320 * Interpolates between two States. 321 * 322 * @param endValue The end value for the interpolation. 323 * @param i The interpolant (fraction). 324 * @return The interpolated state. 325 */ 326 State interpolate(State endValue, double i) { 327 // Find the new t value. 328 final double newT = lerp(timeSeconds, endValue.timeSeconds, i); 329 330 // Find the delta time between the current state and the interpolated state. 331 final double deltaT = newT - timeSeconds; 332 333 // If delta time is negative, flip the order of interpolation. 334 if (deltaT < 0) { 335 return endValue.interpolate(this, 1 - i); 336 } 337 338 // Check whether the robot is reversing at this stage. 339 final boolean reversing = 340 velocityMetersPerSecond < 0 341 || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0; 342 343 // Calculate the new velocity 344 // v_f = v_0 + at 345 final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT); 346 347 // Calculate the change in position. 348 // delta_s = v_0 t + 0.5at² 349 final double newS = 350 (velocityMetersPerSecond * deltaT 351 + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) 352 * (reversing ? -1.0 : 1.0); 353 354 // Return the new state. To find the new position for the new state, we need 355 // to interpolate between the two endpoint poses. The fraction for 356 // interpolation is the change in position (delta s) divided by the total 357 // distance between the two endpoints. 358 final double interpolationFrac = 359 newS / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation()); 360 361 return new State( 362 newT, 363 newV, 364 accelerationMetersPerSecondSq, 365 lerp(poseMeters, endValue.poseMeters, interpolationFrac), 366 lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)); 367 } 368 369 @Override 370 public String toString() { 371 return String.format( 372 "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)", 373 timeSeconds, 374 velocityMetersPerSecond, 375 accelerationMetersPerSecondSq, 376 poseMeters, 377 curvatureRadPerMeter); 378 } 379 380 @Override 381 public boolean equals(Object obj) { 382 if (this == obj) { 383 return true; 384 } 385 if (!(obj instanceof State)) { 386 return false; 387 } 388 State state = (State) obj; 389 return Double.compare(state.timeSeconds, timeSeconds) == 0 390 && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0 391 && Double.compare(state.accelerationMetersPerSecondSq, accelerationMetersPerSecondSq) == 0 392 && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0 393 && Objects.equals(poseMeters, state.poseMeters); 394 } 395 396 @Override 397 public int hashCode() { 398 return Objects.hash( 399 timeSeconds, 400 velocityMetersPerSecond, 401 accelerationMetersPerSecondSq, 402 poseMeters, 403 curvatureRadPerMeter); 404 } 405 } 406 407 @Override 408 public String toString() { 409 String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n")); 410 return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList); 411 } 412 413 @Override 414 public int hashCode() { 415 return m_states.hashCode(); 416 } 417 418 @Override 419 public boolean equals(Object obj) { 420 return obj instanceof Trajectory && m_states.equals(((Trajectory) obj).getStates()); 421 } 422}