Motion Profiling through TrapezoidProfileSubsystems and TrapezoidProfileCommands

Note

For a description of the WPILib motion profiling features used by these command-based wrappers, see Trapezoidal Motion Profiles in WPILib.

Note

The TrapezoidProfile command wrappers are generally intended for composition with custom or external controllers. For combining trapezoidal motion profiling with WPILib’s PIDController, see Combining Motion Profiling and PID in Command-Based.

When controlling a mechanism, is often desirable to move it smoothly between two positions, rather than to abruptly change its setpoint. This is called “motion-profiling,” and is supported in WPILib through the TrapezoidProfile class (Java, C++).

To further help teams integrate motion profiling into their command-based robot projects, WPILib includes two convenience wrappers for the TrapezoidProfile class: TrapezoidProfileSubsystem, which automatically generates and executes motion profiles in its periodic() method, and the TrapezoidProfileCommand, which executes a single user-provided TrapezoidProfile.

TrapezoidProfileSubsystem

Note

In C++, the TrapezoidProfileSubsystem 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.

The TrapezoidProfileSubsystem class (Java, C++) will automatically create and execute trapezoidal motion profiles to reach the user-provided goal state. To use the TrapezoidProfileSubsystem class, users must create a subclass of it.

Creating a TrapezoidProfileSubsystem

When subclassing TrapezoidProfileSubsystem, users must override a single abstract method to provide functionality that the class will use in its ordinary operation:

useState()

protected abstract void useState(TrapezoidProfile.State state);
virtual void UseState(frc::TrapezoidProfile<Distance>::State state) = 0;

The useState() method consumes the current state of the motion profile. The TrapezoidProfileSubsystem will automatically call this method from its periodic() block, and pass it the motion profile state corresponding to the subsystem’s current progress through the motion profile.

Users may do whatever they want with this state; a typical use case (as shown in the Full TrapezoidProfileSubsystem Example) is to use the state to obtain a setpoint and a feedforward for an external “smart” motor controller.

Constructor Parameters

Users must pass in a set of TrapezoidProfile.Constraints to the TrapezoidProfileSubsystem base class through the superclass constructor call of their subclass. This serves to constrain the automatically-generated profiles to a given maximum velocity and acceleration.

Users must also pass in an initial position for the mechanism.

Advanced users may pass in an alternate value for the loop period, if a non-standard main loop period is being used.

Using a TrapezoidProfileSubsystem

Once an instance of a TrapezoidProfileSubsystem subclass has been created, it can be used by commands through the following methods:

setGoal()

Note

If you wish to set the goal to a simple distance with an implicit target velocity of zero, an overload of setGoal() exists that takes a single distance value, rather than a full motion profile state.

The setGoal() method can be used to set the goal state of the TrapezoidProfileSubsystem. The subsystem will automatically execute a profile to the goal, passing the current state at each iteration to the provided useState() method.

// The subsystem will execute a profile to a position of 5 and a velocity of 3.
examplePIDSubsystem.setGoal(new TrapezoidProfile.Goal(5, 3);
// The subsystem will execute a profile to a position of 5 meters and a velocity of 3 mps.
examplePIDSubsyste.SetGoal({5_m, 3_mps});

Full TrapezoidProfileSubsystem Example

What does a TrapezoidProfileSubsystem look like when used in practice? The following examples are taking from the ArmbotOffobard 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
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;

import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;

import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;

/**
 * A robot arm subsystem that moves with a motion profile.
 */
public class ArmSubsystem extends TrapezoidProfileSubsystem {
  private final ExampleSmartMotorController m_motor =
      new ExampleSmartMotorController(ArmConstants.kMotorPort);
  private final ArmFeedforward m_feedforward =
      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);

  /**
   * Create a new ArmSubsystem.
   */
  public ArmSubsystem() {
    super(new TrapezoidProfile.Constraints(ArmConstants.kMaxVelocityRadPerSecond,
                                           ArmConstants.kMaxAccelerationRadPerSecSquared),
          ArmConstants.kArmOffsetRads);
    m_motor.setPID(ArmConstants.kP, 0, 0);
  }

  @Override
  public void useState(TrapezoidProfile.State setpoint) {
    // Calculate the feedforward from the sepoint
    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
    // Add the feedforward to the PID output to get the motor output
    m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, setpoint.position,
                        feedforward / 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
#pragma once

#include <frc/controller/ArmFeedforward.h>
#include <frc2/command/TrapezoidProfileSubsystem.h>
#include <units/angle.h>

#include "ExampleSmartMotorController.h"

/**
 * A robot arm subsystem that moves with a motion profile.
 */
class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
  using State = frc::TrapezoidProfile<units::radians>::State;

 public:
  ArmSubsystem();

  void UseState(State setpoint) override;

 private:
  ExampleSmartMotorController m_motor;
  frc::ArmFeedforward m_feedforward;
};
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
#include "subsystems/ArmSubsystem.h"

#include "Constants.h"

using namespace ArmConstants;
using State = frc::TrapezoidProfile<units::radians>::State;

ArmSubsystem::ArmSubsystem()
    : frc2::TrapezoidProfileSubsystem<units::radians>(
          {kMaxVelocity, kMaxAcceleration}, kArmOffset),
      m_motor(kMotorPort),
      m_feedforward(kS, kCos, kV, kA) {
  m_motor.SetPID(kP, 0, 0);
}

void ArmSubsystem::UseState(State setpoint) {
  // Calculate the feedforward from the sepoint
  units::volt_t feedforward =
      m_feedforward.Calculate(setpoint.position, setpoint.velocity);
  // Add the feedforward to the PID output to get the motor output
  m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
                      setpoint.position.to<double>(), feedforward / 12_V);
}

Using a TrapezoidProfileSubsystem with commands can be quite simple:

63
64
65
66
67
68
69
    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);

    // Move the arm to neutral position when the 'B' button is pressed.
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
33
34
35
36
37
38
39
40
  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
  frc2::JoystickButton(&m_driverController, 1)
      .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});

  // Move the arm to neutral position when the 'B' button is pressed.
  frc2::JoystickButton(&m_driverController, 1)
      .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
                   {&m_arm});

TrapezoidProfileCommand

Note

In C++, the TrapezoidProfileCommand 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.

The TrapezoidProfileCommand class (Java, C++) allows users to create a command that will execute a single TrapezoidProfile, passing its current state at each iteration to a user-defined function.

As with TrapezoidProfileSubsystem, users can create a TrapezoidProfileCommand by subclassing the TrapezoidProfileCommand class. However, as with many of the other command classes in the command-based library, users may want to save code by defining a TrapezoidProfileCommand inline.

Creating a TrapezoidProfileCommand

A TrapezoidProfileCommand can be created two ways - by subclassing the TrapezoidProfileCommand class, or by defining the command inline. Both methods ultimately extremely similar, and ultimately the choice of which to use comes down to where the user desires that the relevant code be located.

In either case, a TrapezoidProfileCommand is created by passing the necessary parameters to its constructor (if defining a subclass, this can be done with a super() call):

28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
  /**
   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
   * Output will be piped to the provided consumer function.
   *
   * @param profile      The motion profile to execute.
   * @param output       The consumer for the profile output.
   * @param requirements The subsystems required by this command.
   */
  public TrapezoidProfileCommand(TrapezoidProfile profile,
                                 Consumer<State> output,
                                 Subsystem... requirements) {
    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
    addRequirements(requirements);
  }
36
37
38
39
40
41
42
43
44
45
46
47
48
 public:
  /**
   * Creates a new TrapezoidProfileCommand that will execute the given
   * TrapezoidalProfile. Output will be piped to the provided consumer function.
   *
   * @param profile The motion profile to execute.
   * @param output  The consumer for the profile output.
   */
  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                          std::function<void(State)> output,
                          std::initializer_list<Subsystem*> requirements)
      : m_profile(profile), m_output(output) {
    this->AddRequirements(requirements);

profile

The profile parameter is the TrapezoidProfile object that will be executed by the command. By passing this in, users specify the start state, end state, and motion constraints of the profile that the command will use.

output

The output parameter is a function (usually passed as a lambda) that consumes the output and setpoint of the control loop. Passing in the useOutput function in PIDCommand is functionally analogous to overriding the useState() function in PIDSubsystem.

requirements

Like all inlineable commands, TrapezoidProfileCommand allows the user to specify its subsystem requirements as a constructor parameter.

Full TrapezoidProfileCommand Example

What does a TrapezoidProfileSubsystem look like when used in practice? The following examples are taking from the DriveDistanceOffboard 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
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;

import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;

import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;

/**
 * Drives a set distance using a motion profile.
 */
public class DriveDistanceProfiled extends TrapezoidProfileCommand {
  /**
   * Creates a new DriveDistanceProfiled command.
   *
   * @param meters The distance to drive.
   * @param drive The drive subsystem to use.
   */
  public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
    super(
        new TrapezoidProfile(
            // Limit the max acceleration and velocity
            new TrapezoidProfile.Constraints(DriveConstants.kMaxSpeedMetersPerSecond,
                                             DriveConstants.kMaxAccelerationMetersPerSecondSquared),
            // End at desired position in meters; implicitly starts at 0
            new TrapezoidProfile.State(meters, 0)),
        // Pipe the profile state to the drive
        setpointState -> drive.setDriveStates(setpointState, setpointState),
        // Require the drive
        drive);
    // Reset drive encoders since we're starting at 0
    drive.resetEncoders();
  }
}
 8
 9
10
11
12
13
14
15
16
17
18
19
20
#pragma once

#include <frc2/command/CommandHelper.h>
#include <frc2/command/TrapezoidProfileCommand.h>

#include "subsystems/DriveSubsystem.h"

class DriveDistanceProfiled
    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
                                 DriveDistanceProfiled> {
 public:
  DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
};
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
#include "commands/DriveDistanceProfiled.h"

#include "Constants.h"

using namespace DriveConstants;

DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
                                             DriveSubsystem* drive)
    : CommandHelper(
          frc::TrapezoidProfile<units::meters>(
              // Limit the max acceleration and velocity
              {kMaxSpeed, kMaxAcceleration},
              // End at desired position in meters; implicitly starts at 0
              {distance, 0_mps}),
          // Pipe the profile state to the drive
          [drive](auto setpointState) {
            drive->SetDriveStates(setpointState, setpointState);
          },
          // Require the drive
          {drive}) {
  // Reset drive encoders since we're starting at 0
  drive->ResetEncoders();
}

And, for an inlined example:

68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
    // Do the same thing as above when the 'B' button is pressed, but defined inline
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(
            new TrapezoidProfileCommand(
                new TrapezoidProfile(
                    // Limit the max acceleration and velocity
                    new TrapezoidProfile.Constraints(
                        DriveConstants.kMaxSpeedMetersPerSecond,
                        DriveConstants.kMaxAccelerationMetersPerSecondSquared),
                    // End at desired position in meters; implicitly starts at 0
                    new TrapezoidProfile.State(3, 0)),
                // Pipe the profile state to the drive
                setpointState -> m_robotDrive.setDriveStates(
                    setpointState,
                    setpointState),
                // Require the drive
                m_robotDrive)
                .beforeStarting(m_robotDrive::resetEncoders)
                .withTimeout(10));
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
  // Do the same thing as above when the 'B' button is pressed, but defined
  // inline
  frc2::JoystickButton(&m_driverController, 2)
      .WhenPressed(
          frc2::TrapezoidProfileCommand<units::meters>(
              frc::TrapezoidProfile<units::meters>(
                  // Limit the max acceleration and velocity
                  {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
                  // End at desired position in meters; implicitly starts at 0
                  {3_m, 0_mps}),
              // Pipe the profile state to the drive
              [this](auto setpointState) {
                m_drive.SetDriveStates(setpointState, setpointState);
              },
              // Require the drive
              {&m_drive})
              .BeforeStarting([this]() { m_drive.ResetEncoders(); })
              .WithTimeout(10_s));
}