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 static edu.wpi.first.units.Units.MetersPerSecond;
008import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
009
010import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
011import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
012import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
013import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
014import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint;
015import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
016import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
017import edu.wpi.first.units.measure.LinearAcceleration;
018import edu.wpi.first.units.measure.LinearVelocity;
019import java.util.ArrayList;
020import java.util.List;
021
022/**
023 * Represents the configuration for generating a trajectory. This class stores the start velocity,
024 * end velocity, max velocity, max acceleration, custom constraints, and the reversed flag.
025 *
026 * <p>The class must be constructed with a max velocity and max acceleration. The other parameters
027 * (start velocity, end velocity, constraints, reversed) have been defaulted to reasonable values
028 * (0, 0, {}, false). These values can be changed via the setXXX methods.
029 */
030public class TrajectoryConfig {
031  private final double m_maxVelocity;
032  private final double m_maxAcceleration;
033  private final List<TrajectoryConstraint> m_constraints;
034  private double m_startVelocity;
035  private double m_endVelocity;
036  private boolean m_reversed;
037
038  /**
039   * Constructs the trajectory configuration class.
040   *
041   * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
042   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
043   */
044  public TrajectoryConfig(
045      double maxVelocityMetersPerSecond, double maxAccelerationMetersPerSecondSq) {
046    m_maxVelocity = maxVelocityMetersPerSecond;
047    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
048    m_constraints = new ArrayList<>();
049  }
050
051  /**
052   * Constructs the trajectory configuration class.
053   *
054   * @param maxVelocity The max velocity for the trajectory.
055   * @param maxAcceleration The max acceleration for the trajectory.
056   */
057  public TrajectoryConfig(LinearVelocity maxVelocity, LinearAcceleration maxAcceleration) {
058    this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond));
059  }
060
061  /**
062   * Adds a user-defined constraint to the trajectory.
063   *
064   * @param constraint The user-defined constraint.
065   * @return Instance of the current config object.
066   */
067  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
068    m_constraints.add(constraint);
069    return this;
070  }
071
072  /**
073   * Adds all user-defined constraints from a list to the trajectory.
074   *
075   * @param constraints List of user-defined constraints.
076   * @return Instance of the current config object.
077   */
078  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
079    m_constraints.addAll(constraints);
080    return this;
081  }
082
083  /**
084   * Adds a differential drive kinematics constraint to ensure that no wheel velocity of a
085   * differential drive goes above the max velocity.
086   *
087   * @param kinematics The differential drive kinematics.
088   * @return Instance of the current config object.
089   */
090  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
091    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
092    return this;
093  }
094
095  /**
096   * Adds a mecanum drive kinematics constraint to ensure that no wheel velocity of a mecanum drive
097   * goes above the max velocity.
098   *
099   * @param kinematics The mecanum drive kinematics.
100   * @return Instance of the current config object.
101   */
102  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
103    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
104    return this;
105  }
106
107  /**
108   * Adds a swerve drive kinematics constraint to ensure that no wheel velocity of a swerve drive
109   * goes above the max velocity.
110   *
111   * @param kinematics The swerve drive kinematics.
112   * @return Instance of the current config object.
113   */
114  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
115    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
116    return this;
117  }
118
119  /**
120   * Returns the starting velocity of the trajectory.
121   *
122   * @return The starting velocity of the trajectory.
123   */
124  public double getStartVelocity() {
125    return m_startVelocity;
126  }
127
128  /**
129   * Sets the start velocity of the trajectory.
130   *
131   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
132   * @return Instance of the current config object.
133   */
134  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
135    m_startVelocity = startVelocityMetersPerSecond;
136    return this;
137  }
138
139  /**
140   * Sets the start velocity of the trajectory.
141   *
142   * @param startVelocity The start velocity of the trajectory.
143   * @return Instance of the current config object.
144   */
145  public TrajectoryConfig setStartVelocity(LinearVelocity startVelocity) {
146    return setStartVelocity(startVelocity.in(MetersPerSecond));
147  }
148
149  /**
150   * Returns the starting velocity of the trajectory.
151   *
152   * @return The starting velocity of the trajectory.
153   */
154  public double getEndVelocity() {
155    return m_endVelocity;
156  }
157
158  /**
159   * Sets the end velocity of the trajectory.
160   *
161   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
162   * @return Instance of the current config object.
163   */
164  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
165    m_endVelocity = endVelocityMetersPerSecond;
166    return this;
167  }
168
169  /**
170   * Sets the end velocity of the trajectory.
171   *
172   * @param endVelocity The end velocity of the trajectory.
173   * @return Instance of the current config object.
174   */
175  public TrajectoryConfig setEndVelocity(LinearVelocity endVelocity) {
176    return setEndVelocity(endVelocity.in(MetersPerSecond));
177  }
178
179  /**
180   * Returns the maximum velocity of the trajectory.
181   *
182   * @return The maximum velocity of the trajectory.
183   */
184  public double getMaxVelocity() {
185    return m_maxVelocity;
186  }
187
188  /**
189   * Returns the maximum acceleration of the trajectory.
190   *
191   * @return The maximum acceleration of the trajectory.
192   */
193  public double getMaxAcceleration() {
194    return m_maxAcceleration;
195  }
196
197  /**
198   * Returns the user-defined constraints of the trajectory.
199   *
200   * @return The user-defined constraints of the trajectory.
201   */
202  public List<TrajectoryConstraint> getConstraints() {
203    return m_constraints;
204  }
205
206  /**
207   * Returns whether the trajectory is reversed or not.
208   *
209   * @return whether the trajectory is reversed or not.
210   */
211  public boolean isReversed() {
212    return m_reversed;
213  }
214
215  /**
216   * Sets the reversed flag of the trajectory.
217   *
218   * @param reversed Whether the trajectory should be reversed or not.
219   * @return Instance of the current config object.
220   */
221  public TrajectoryConfig setReversed(boolean reversed) {
222    m_reversed = reversed;
223    return this;
224  }
225}