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 through TrapezoidProfileSubsystems and TrapezoidProfileCommands.
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++).
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++):
// 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};
Start and End States¶
Next, we must specify the desired starting and ending states for our mechanisms using the TrapezoidProfile.State
class (Java, C++). 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};
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 3 parameters, in order: the constraints, the goal state, and the initial state.
// 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 will end stationary at 5 meters
// Profile will start stationary at zero position
TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(5, 10),
new TrapezoidProfile.State(5, 0),
new TrapezoidProfile.State(0, 0));
// 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 will end stationary at 5 meters
// Profile will start stationary at zero position
frc::TrapezoidProfile<units::meters> profile(
frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq},
frc::TrapezoidProfile<units::meters>::State{5_m, 0_mps},
frc::TrapezoidProfile<units::meters>::State(0_m, 0_mps});
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:
// Returns the motion profile state after 5 seconds of motion
profile.calculate(5);
// Returns the motion profile state after 5 seconds of motion
profile.Calculate(5_s);
Using the State¶
The calculate
method returns a TrapezoidProfile.State
class (the same one that was used to specify the initial/end states when constructing the profile). 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);
controller.calculate(encoder.getDistance(), setpoint.position);
auto setpoint = profile.Calculate(elapsedTime);
controller.Calculate(encoder.GetDistance(), setpoint.position.to<double>());
Complete Usage Example¶
Note
In this example, the profile 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 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++):
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 | package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
public class Robot extends TimedRobot {
private static double kDt = 0.02;
private final Joystick m_joystick = new Joystick(1);
private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
// Note: These gains are fake, and will have to be tuned for your robot.
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
private final TrapezoidProfile.Constraints m_constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
@Override
public void robotInit() {
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.setPID(1.3, 0.0, 0.7);
}
@Override
public void teleopPeriodic() {
if (m_joystick.getRawButtonPressed(2)) {
m_goal = new TrapezoidProfile.State(5, 0);
} else if (m_joystick.getRawButtonPressed(3)) {
m_goal = new TrapezoidProfile.State(0, 0);
}
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.calculate(kDt);
// Send setpoint to offboard controller PID
m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity) / 12.0);
}
}
|
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 | #include <frc/Joystick.h>
#include <frc/TimedRobot.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <units/units.h>
#include <wpi/math>
#include "ExampleSmartMotorController.h"
class Robot : public frc::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
m_motor.SetPID(1.3, 0.0, 0.7);
}
void TeleopPeriodic() override {
if (m_joystick.GetRawButtonPressed(2)) {
m_goal = {5_m, 0_mps};
} else if (m_joystick.GetRawButtonPressed(3)) {
m_goal = {0_m, 0_mps};
}
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
m_setpoint};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.Calculate(kDt);
// Send setpoint to offboard controller PID
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
m_setpoint.position.to<double>(),
m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
}
private:
frc::Joystick m_joystick{1};
ExampleSmartMotorController m_motor{1};
frc::SimpleMotorFeedforward<units::meters> m_feedforward{
// Note: These gains are fake, and will have to be tuned for your robot.
1_V, 1.5_V * 1_s / 1_m};
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meters>::State m_goal;
frc::TrapezoidProfile<units::meters>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif
|