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