- FRC Game Manual
- FRC Game Q&A

# Trapezoidal Motion Profiles in WPILib

Note

This article covers the in-code generation of trapezoidal motion profiles. Documentation describing the involved concepts in more detail is forthcoming.

Note

For a guide on implementing the `TrapezoidProfile` class in the command-based framework framework, see Motion Profiling in Command-based.

Note

The `TrapezoidProfile` class, used on its own, is most useful when composed with a custom controller (such as a “smart” motor controller with a built-in PID functionality). To integrate it with a WPILib `PIDController`, see Combining Motion Profiling and PID Control with ProfiledPIDController.

While feedforward and feedback control offer convenient ways to achieve a given setpoint, we are often still faced with the problem of generating setpoints for our mechanisms. While the naive approach of immediately commanding a mechanism to its desired state may work, it is often suboptimal. To improve the handling of our mechanisms, we often wish to command mechanisms to a sequence of setpoints that smoothly interpolate between its current state, and its desired goal state.

To help users do this, WPILib provides a `TrapezoidProfile` class (Java, C++, `Python`).

## Creating a TrapezoidProfile

Note

In C++, the `TrapezoidProfile` class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see The C++ Units Library.

### Constraints

Note

The various feedforward helper classes provide methods for calculating the maximum simultaneously-achievable velocity and acceleration of a mechanism. These can be very useful for calculating appropriate motion constraints for your `TrapezoidProfile`.

In order to create a trapezoidal motion profile, we must first impose some constraints on the desired motion. Namely, we must specify a maximum velocity and acceleration that the mechanism will be expected to achieve during the motion. To do this, we create an instance of the `TrapezoidProfile.Constraints` class (Java, C++, `Python`):

JAVA

    // Creates a new set of trapezoidal motion profile constraints
    // Max velocity of 10 meters per second
    // Max acceleration of 20 meters per second squared
    new TrapezoidProfile.Constraints(10, 20);

C++

    // Creates a new set of trapezoidal motion profile constraints
    // Max velocity of 10 meters per second
    // Max acceleration of 20 meters per second squared
    frc::TrapezoidProfile<units::meters>::Constraints{10_mps, 20_mps_sq};

PYTHON

    from wpimath.trajectory import TrapezoidProfile
    # Creates a new set of trapezoidal motion profile constraints
    # Max velocity of 10 meters per second
    # Max acceleration of 20 meters per second squared
    TrapezoidProfile.Constraints(10, 20)

### Start and End States

Next, we must specify the desired starting and ending states for our mechanisms using the `TrapezoidProfile.State` class (Java, C++, `Python`). Each state has a position and a velocity:

JAVA

    // Creates a new state with a position of 5 meters
    // and a velocity of 0 meters per second
    new TrapezoidProfile.State(5, 0);

C++

    // Creates a new state with a position of 5 meters
    // and a velocity of 0 meters per second
    frc::TrapezoidProfile<units::meters>::State{5_m, 0_mps};

PYTHON

    from wpimath.trajectory import TrapezoidProfile
    # Creates a new state with a position of 5 meters
    # and a velocity of 0 meters per second
    TrapezoidProfile.State(5, 0)

### Putting It All Together

Note

C++ is often able to infer the type of the inner classes, and thus a simple initializer list (without the class name) can be sent as a parameter. The full class names are included in the example below for clarity.

Now that we know how to create a set of constraints and the desired start/end states, we are ready to create our motion profile. The `TrapezoidProfile` constructor takes 1 parameter: the constraints.

JAVA

    // Creates a new TrapezoidProfile
    // Profile will have a max vel of 5 meters per second
    // Profile will have a max acceleration of 10 meters per second squared
    TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(5, 10));

C++

    // Creates a new TrapezoidProfile
    // Profile will have a max vel of 5 meters per second
    // Profile will have a max acceleration of 10 meters per second squared
    frc::TrapezoidProfile<units::meters> profile{
      frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq}};

PYTHON

    from wpimath.trajectory import TrapezoidProfile
    # Creates a new TrapezoidProfile
    # Profile will have a max vel of 5 meters per second
    # Profile will have a max acceleration of 10 meters per second squared
    profile = TrapezoidProfile(TrapezoidProfile.Constraints(5, 10))

## Using a `TrapezoidProfile`

### Sampling the Profile

Once we’ve created a `TrapezoidProfile`, using it is very simple: to get the profile state at the given time after the profile has started, call the `calculate()` method with the goal state and initial state:

JAVA

    // Profile will start stationary at zero position
    // Profile will end stationary at 5 meters
    // Returns the motion profile state after 5 seconds of motion
    profile.calculate(5, new TrapezoidProfile.State(0, 0), new TrapezoidProfile.State(5, 0));

C++

    // Profile will start stationary at zero position
    // Profile will end stationary at 5 meters
    // Returns the motion profile state after 5 seconds of motion
    profile.Calculate(5_s,
    frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps},
    frc::TrapezoidProfile<units::meters>::State{5_m, 0_mps});

PYTHON

    # Profile will start stationary at zero position
    # Profile will end stationary at 5 meters
    # Returns the motion profile state after 5 seconds of motion
    profile.calculate(5, TrapezoidProfile.State(0, 0), TrapezoidProfile.State(5, 0))

### Using the State

The `calculate` method returns a `TrapezoidProfile.State` class (the same one that was used to specify the initial/end states when calculating the profile state). To use this for actual control, simply pass the contained position and velocity values to whatever controller you wish (for example, a PIDController):

JAVA

    var setpoint = profile.calculate(elapsedTime, initialState, goalState);
    controller.calculate(encoder.getDistance(), setpoint.position);

C++

    auto setpoint = profile.Calculate(elapsedTime, initialState, goalState);
    controller.Calculate(encoder.GetDistance(), setpoint.position.value());

PYTHON

    setpoint = profile.calculate(elapsedTime, initialState, goalState)
    controller.calculate(encoder.getDistance(), setpoint.position)

## Complete Usage Example

Note

In this example, the initial state is re-computed every timestep. This is a somewhat different usage technique than is detailed above, but works according to the same principles - the profile is sampled at a time corresponding to the loop period to get the setpoint for the next loop iteration.

A more complete example of `TrapezoidProfile` usage is provided in the ElevatorTrapezoidProfile example project (Java, C++, Python):

JAVA

     5package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
     6
     7import edu.wpi.first.math.controller.SimpleMotorFeedforward;
     8import edu.wpi.first.math.trajectory.TrapezoidProfile;
     9import edu.wpi.first.wpilibj.Joystick;
    10import edu.wpi.first.wpilibj.TimedRobot;
    11
    12public class Robot extends TimedRobot {
    13  private static double kDt = 0.02;
    14
    15  private final Joystick m_joystick = new Joystick(1);
    16  private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
    17  // Note: These gains are fake, and will have to be tuned for your robot.
    18  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
    19
    20  // Create a motion profile with the given maximum velocity and maximum
    21  // acceleration constraints for the next setpoint.
    22  private final TrapezoidProfile m_profile =
    23      new TrapezoidProfile(new TrapezoidProfile.Constraints(1.75, 0.75));
    24  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
    25  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
    26
    27  public Robot() {
    28    // Note: These gains are fake, and will have to be tuned for your robot.
    29    m_motor.setPID(1.3, 0.0, 0.7);
    30  }
    31
    32  @Override
    33  public void teleopPeriodic() {
    34    if (m_joystick.getRawButtonPressed(2)) {
    35      m_goal = new TrapezoidProfile.State(5, 0);
    36    } else if (m_joystick.getRawButtonPressed(3)) {
    37      m_goal = new TrapezoidProfile.State();
    38    }
    39
    40    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
    41    // toward the goal while obeying the constraints.
    42    m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
    43
    44    // Send setpoint to offboard controller PID
    45    m_motor.setSetpoint(
    46        ExampleSmartMotorController.PIDMode.kPosition,
    47        m_setpoint.position,
    48        m_feedforward.calculate(m_setpoint.velocity) / 12.0);
    49  }
    50}

C++

     5#include <numbers>
     6
     7#include <frc/Joystick.h>
     8#include <frc/TimedRobot.h>
     9#include <frc/controller/SimpleMotorFeedforward.h>
    10#include <frc/trajectory/TrapezoidProfile.h>
    11#include <units/acceleration.h>
    12#include <units/length.h>
    13#include <units/time.h>
    14#include <units/velocity.h>
    15#include <units/voltage.h>
    16
    17#include "ExampleSmartMotorController.h"
    18
    19class Robot : public frc::TimedRobot {
    20 public:
    21  static constexpr units::second_t kDt = 20_ms;
    22
    23  Robot() {
    24    // Note: These gains are fake, and will have to be tuned for your robot.
    25    m_motor.SetPID(1.3, 0.0, 0.7);
    26  }
    27
    28  void TeleopPeriodic() override {
    29    if (m_joystick.GetRawButtonPressed(2)) {
    30      m_goal = {5_m, 0_mps};
    31    } else if (m_joystick.GetRawButtonPressed(3)) {
    32      m_goal = {0_m, 0_mps};
    33    }
    34
    35    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
    36    // toward the goal while obeying the constraints.
    37    m_setpoint = m_profile.Calculate(kDt, m_setpoint, m_goal);
    38
    39    // Send setpoint to offboard controller PID
    40    m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
    41                        m_setpoint.position.value(),
    42                        m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
    43  }
    44
    45 private:
    46  frc::Joystick m_joystick{1};
    47  ExampleSmartMotorController m_motor{1};
    48  frc::SimpleMotorFeedforward<units::meters> m_feedforward{
    49      // Note: These gains are fake, and will have to be tuned for your robot.
    50      1_V, 1.5_V * 1_s / 1_m};
    51
    52  // Create a motion profile with the given maximum velocity and maximum
    53  // acceleration constraints for the next setpoint.
    54  frc::TrapezoidProfile<units::meters> m_profile{{1.75_mps, 0.75_mps_sq}};
    55  frc::TrapezoidProfile<units::meters>::State m_goal;
    56  frc::TrapezoidProfile<units::meters>::State m_setpoint;
    57};
    58
    59#ifndef RUNNING_FRC_TESTS
    60int main() {
    61  return frc::StartRobot<Robot>();
    62}
    63#endif

PYTHON

     8import wpilib
     9import wpimath.controller
    10from wpimath.trajectory import TrapezoidProfile
    11
    12import examplesmartmotorcontroller
    13
    14
    15class MyRobot(wpilib.TimedRobot):
    16    kDt = 0.02
    17
    18    def robotInit(self):
    19        self.joystick = wpilib.Joystick(1)
    20        self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1)
    21        # Note: These gains are fake, and will have to be tuned for your robot.
    22        self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5)
    23
    24        # Create a motion profile with the given maximum velocity and maximum
    25        # acceleration constraints for the next setpoint.
    26        self.profile = TrapezoidProfile(TrapezoidProfile.Constraints(1.75, 0.75))
    27
    28        self.goal = TrapezoidProfile.State()
    29        self.setpoint = TrapezoidProfile.State()
    30
    31        # Note: These gains are fake, and will have to be tuned for your robot.
    32        self.motor.setPID(1.3, 0.0, 0.7)
    33
    34    def teleopPeriodic(self):
    35        if self.joystick.getRawButtonPressed(2):
    36            self.goal = TrapezoidProfile.State(5, 0)
    37        elif self.joystick.getRawButtonPressed(3):
    38            self.goal = TrapezoidProfile.State(0, 0)
    39
    40        # Retrieve the profiled setpoint for the next timestep. This setpoint moves
    41        # toward the goal while obeying the constraints.
    42        self.setpoint = self.profile.calculate(self.kDt, self.setpoint, self.goal)
    43
    44        # Send setpoint to offboard controller PID
    45        self.motor.setSetPoint(
    46            examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
    47            self.setpoint.position,
    48            self.feedforward.calculate(self.setpoint.velocity) / 12,
    49        )