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