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