Trapezoidal Motion Profiles in WPILib#
הערה
This article covers the in-code generation of trapezoidal motion profiles. Documentation describing the involved concepts in more detail is forthcoming.
הערה
For a guide on implementing the TrapezoidProfile
class in the command-based framework framework, see Motion Profiling through TrapezoidProfileSubsystems and TrapezoidProfileCommands.
הערה
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#
הערה
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#
הערה
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
):
// 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);
// 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};
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:
// Creates a new state with a position of 5 meters
// and a velocity of 0 meters per second
new TrapezoidProfile.State(5, 0);
// 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};
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#
הערה
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.
// 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));
// 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}};
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:
// 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));
// 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});
# 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):
var setpoint = profile.calculate(elapsedTime, initialState, goalState);
controller.calculate(encoder.getDistance(), setpoint.position);
auto setpoint = profile.Calculate(elapsedTime, initialState, goalState);
controller.Calculate(encoder.GetDistance(), setpoint.position.value());
setpoint = profile.calculate(elapsedTime, initialState, goalState)
controller.calculate(encoder.getDistance(), setpoint.position)
Complete Usage Example#
הערה
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):
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 @Override
28 public void robotInit() {
29 // Note: These gains are fake, and will have to be tuned for your robot.
30 m_motor.setPID(1.3, 0.0, 0.7);
31 }
32
33 @Override
34 public void teleopPeriodic() {
35 if (m_joystick.getRawButtonPressed(2)) {
36 m_goal = new TrapezoidProfile.State(5, 0);
37 } else if (m_joystick.getRawButtonPressed(3)) {
38 m_goal = new TrapezoidProfile.State();
39 }
40
41 // Retrieve the profiled setpoint for the next timestep. This setpoint moves
42 // toward the goal while obeying the constraints.
43 m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
44
45 // Send setpoint to offboard controller PID
46 m_motor.setSetpoint(
47 ExampleSmartMotorController.PIDMode.kPosition,
48 m_setpoint.position,
49 m_feedforward.calculate(m_setpoint.velocity) / 12.0);
50 }
51}
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
8import wpilib
9import wpimath.controller
10import wpimath.trajectory
11import examplesmartmotorcontroller
12
13
14class MyRobot(wpilib.TimedRobot):
15 kDt = 0.02
16
17 def robotInit(self):
18 self.joystick = wpilib.Joystick(1)
19 self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1)
20 # Note: These gains are fake, and will have to be tuned for your robot.
21 self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5)
22
23 self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75)
24
25 self.goal = wpimath.trajectory.TrapezoidProfile.State()
26 self.setpoint = wpimath.trajectory.TrapezoidProfile.State()
27
28 # Note: These gains are fake, and will have to be tuned for your robot.
29 self.motor.setPID(1.3, 0.0, 0.7)
30
31 def teleopPeriodic(self):
32 if self.joystick.getRawButtonPressed(2):
33 self.goal = wpimath.trajectory.TrapezoidProfile.State(5, 0)
34 elif self.joystick.getRawButtonPressed(3):
35 self.goal = wpimath.trajectory.TrapezoidProfile.State(0, 0)
36
37 # Create a motion profile with the given maximum velocity and maximum
38 # acceleration constraints for the next setpoint, the desired goal, and the
39 # current setpoint.
40 profile = wpimath.trajectory.TrapezoidProfile(
41 self.constraints, self.goal, self.setpoint
42 )
43
44 # Retrieve the profiled setpoint for the next timestep. This setpoint moves
45 # toward the goal while obeying the constraints.
46 self.setpoint = profile.calculate(self.kDt)
47
48 # Send setpoint to offboard controller PID
49 self.motor.setSetPoint(
50 examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
51 self.setpoint.position,
52 self.feedforward.calculate(self.setpoint.velocity) / 12,
53 )