[wpimath] Refactor TrapezoidProfile API (#5457)

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Consumer;
+import java.util.function.Supplier;
 
 /**
  * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion.
@@ -19,21 +20,53 @@
 public class TrapezoidProfileCommand extends Command {
   private final TrapezoidProfile m_profile;
   private final Consumer<State> m_output;
-
+  private final Supplier<State> m_goal;
+  private final Supplier<State> m_currentState;
+  private final boolean m_newAPI; // TODO: Remove
   private final Timer m_timer = new Timer();
 
+  /**
+   * 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 goal The supplier for the desired state
+   * @param currentState The current state
+   * @param requirements The subsystems required by this command.
+   */
+  public TrapezoidProfileCommand(
+      TrapezoidProfile profile,
+      Consumer<State> output,
+      Supplier<State> goal,
+      Supplier<State> currentState,
+      Subsystem... requirements) {
+    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
+    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
+    m_goal = goal;
+    m_currentState = currentState;
+    m_newAPI = true;
+    addRequirements(requirements);
+  }
+
   /**
    * 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.
+   * @deprecated The new constructor allows you to pass in a supplier for desired and current state.
+   *     This allows you to change goals at runtime.
    */
+  @Deprecated(since = "2024", forRemoval = true)
   public TrapezoidProfileCommand(
       TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
     m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
     m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
+    m_newAPI = false;
+    m_goal = null;
+    m_currentState = null;
     addRequirements(requirements);
   }
 
@@ -43,8 +76,13 @@ public void initialize() {
   }
 
   @Override
+  @SuppressWarnings("removal")
   public void execute() {
-    m_output.accept(m_profile.calculate(m_timer.get()));
+    if (m_newAPI) {
+      m_output.accept(m_profile.calculate(m_timer.get(), m_goal.get(), m_currentState.get()));
+    } else {
+      m_output.accept(m_profile.calculate(m_timer.get()));
+    }
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java

@@ -16,7 +16,7 @@
  */
 public abstract class TrapezoidProfileSubsystem extends Subsystem {
   private final double m_period;
-  private final TrapezoidProfile.Constraints m_constraints;
+  private final TrapezoidProfile m_profile;
 
   private TrapezoidProfile.State m_state;
   private TrapezoidProfile.State m_goal;
@@ -33,7 +33,8 @@ public abstract class TrapezoidProfileSubsystem extends Subsystem {
    */
   public TrapezoidProfileSubsystem(
       TrapezoidProfile.Constraints constraints, double initialPosition, double period) {
-    m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
+    requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
+    m_profile = new TrapezoidProfile(constraints);
     m_state = new TrapezoidProfile.State(initialPosition, 0);
     setGoal(initialPosition);
     m_period = period;
@@ -62,8 +63,7 @@ public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
 
   @Override
   public void periodic() {
-    var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
-    m_state = profile.calculate(m_period);
+    m_state = m_profile.calculate(m_period, m_goal, m_state);
     if (m_enabled) {
       useState(m_state);
     }

wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h

@@ -39,13 +39,65 @@ class TrapezoidProfileCommand
    *
    * @param profile      The motion profile to execute.
    * @param output       The consumer for the profile output.
+   * @param goal The supplier for the desired state
+   * @param currentState The current state
    * @param requirements The list of requirements.
    */
+  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
+                          std::function<void(State)> output,
+                          std::function<State()> goal,
+                          std::function<State()> currentState,
+                          std::initializer_list<Subsystem*> requirements)
+      : m_profile(profile),
+        m_output(output),
+        m_goal(goal),
+        m_currentState(currentState) {
+    this->AddRequirements(requirements);
+    m_newAPI = true;
+  }
+
+  /**
+   * 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.
+   * @param goal The supplier for the desired state
+   * @param currentState The current state
+   * @param requirements The list of requirements.
+   */
+  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
+                          std::function<void(State)> output,
+                          std::function<State()> goal,
+                          std::function<State()> currentState,
+                          std::span<Subsystem* const> requirements = {})
+      : m_profile(profile),
+        m_output(output),
+        m_goal(goal),
+        m_currentState(currentState) {
+    this->AddRequirements(requirements);
+    m_newAPI = true;
+  }
+
+  /**
+   * 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.
+   * @param requirements The list of requirements.
+   * @deprecated The new constructor allows you to pass in a supplier for
+   * desired and current state. This allows you to change goals at runtime.
+   */
+  WPI_DEPRECATED(
+      "The new constructor allows you to pass in a supplier for desired and "
+      "current state. This allows you to change goals at runtime.")
   TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                           std::function<void(State)> output,
                           std::initializer_list<Subsystem*> requirements)
       : m_profile(profile), m_output(output) {
     this->AddRequirements(requirements);
+    m_newAPI = false;
   }
 
   /**
@@ -55,17 +107,25 @@ class TrapezoidProfileCommand
    * @param profile      The motion profile to execute.
    * @param output       The consumer for the profile output.
    * @param requirements The list of requirements.
+   * @deprecated The new constructor allows you to pass in a supplier for
+   * desired and current state. This allows you to change goals at runtime.
    */
+  WPI_DEPRECATED(
+      "The new constructor allows you to pass in a supplier for desired and "
+      "current state. This allows you to change goals at runtime.")
   TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                           std::function<void(State)> output,
                           std::span<Subsystem* const> requirements = {})
       : m_profile(profile), m_output(output) {
     this->AddRequirements(requirements);
+    m_newAPI = false;
   }
 
   void Initialize() override { m_timer.Restart(); }
 
-  void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); }
+  void Execute() override {
+    m_output(m_profile.Calculate(m_timer.Get(), m_goal(), m_currentState()));
+  }
 
   void End(bool interrupted) override { m_timer.Stop(); }
 
@@ -76,7 +136,9 @@ class TrapezoidProfileCommand
  private:
   frc::TrapezoidProfile<Distance> m_profile;
   std::function<void(State)> m_output;
-
+  std::function<State()> m_goal;
+  std::function<State()> m_currentState;
+  bool m_newAPI;  // TODO: Remove
   frc::Timer m_timer;
 };
 

wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h

@@ -39,15 +39,13 @@ class TrapezoidProfileSubsystem : public Subsystem {
   explicit TrapezoidProfileSubsystem(Constraints constraints,
                                      Distance_t initialPosition = Distance_t{0},
                                      units::second_t period = 20_ms)
-      : m_constraints(constraints),
+      : m_profile(constraints),
         m_state{initialPosition, Velocity_t(0)},
         m_goal{initialPosition, Velocity_t{0}},
         m_period(period) {}
 
   void Periodic() override {
-    auto profile =
-        frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
-    m_state = profile.Calculate(m_period);
+    m_state = m_profile.Calculate(m_period, m_goal, m_state);
     if (m_enabled) {
       UseState(m_state);
     }
@@ -87,7 +85,7 @@ class TrapezoidProfileSubsystem : public Subsystem {
   void Disable() { m_enabled = false; }
 
  private:
-  Constraints m_constraints;
+  frc::TrapezoidProfile<Distance> m_profile;
   State m_state;
   State m_goal;
   units::second_t m_period;

wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp

@@ -13,13 +13,18 @@
 ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand()
     : CommandHelper
       // The profile to execute
-      (frc::TrapezoidProfile<units::meters>(
-           // The maximum velocity and acceleration of the profile
-           {5_mps, 5_mps_sq},
-           // The goal state of the profile
-           {10_m, 0_mps},
-           // The initial state of the profile
-           {0_m, 0_mps}),
-       [](frc::TrapezoidProfile<units::meters>::State state) {
-         // Use the computed intermediate trajectory state here
-       }) {}
+      (
+          frc::TrapezoidProfile<units::meters>(
+              // The maximum velocity and acceleration of the profile
+              {5_mps, 5_mps_sq}),
+          [](frc::TrapezoidProfile<units::meters>::State state) {
+            // Use the computed intermediate trajectory state here
+          },
+          // The goal state of the profile
+          [] {
+            return frc::TrapezoidProfile<units::meters>::State{10_m, 0_mps};
+          },
+          // The initial state of the profile
+          [] {
+            return frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps};
+          }) {}

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp

@@ -40,13 +40,17 @@ void RobotContainer::ConfigureButtonBindings() {
       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}),
+              {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}),
           // Pipe the profile state to the drive
           [this](auto setpointState) {
             m_drive.SetDriveStates(setpointState, setpointState);
           },
+          // End at desired position in meters; implicitly starts at 0
+          [] {
+            return frc::TrapezoidProfile<units::meters>::State{3_m, 0_mps};
+          },
+          // Current position
+          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
           // Require the drive
           {&m_drive})
           // Convert to CommandPtr

wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp

@@ -13,13 +13,16 @@ DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
     : 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}},
+              {kMaxSpeed, kMaxAcceleration}},
           // Pipe the profile state to the drive
           [drive](auto setpointState) {
             drive->SetDriveStates(setpointState, setpointState);
           },
+          // End at desired position in meters; implicitly starts at 0
+          [distance] {
+            return frc::TrapezoidProfile<units::meters>::State{distance, 0_mps};
+          },
+          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
           // Require the drive
           {drive}} {
   // Reset drive encoders since we're starting at 0

wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp

@@ -33,14 +33,12 @@ class Robot : public frc::TimedRobot {
     }
 
     // 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::meters> profile{m_constraints, m_goal,
-                                                 m_setpoint};
+    // acceleration constraints for the next setpoint.
+    frc::TrapezoidProfile<units::meters> profile{m_constraints};
 
     // Retrieve the profiled setpoint for the next timestep. This setpoint moves
     // toward the goal while obeying the constraints.
-    m_setpoint = profile.Calculate(kDt);
+    m_setpoint = profile.Calculate(kDt, m_goal, m_setpoint);
 
     // Send setpoint to offboard controller PID
     m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,

wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp

@@ -86,8 +86,8 @@ class Robot : public frc::TimedRobot {
   frc::PWMSparkMax m_motor{kMotorPort};
   frc::XboxController m_joystick{kJoystickPort};
 
-  frc::TrapezoidProfile<units::radians>::Constraints m_constraints{
-      45_deg_per_s, 90_deg_per_s / 1_s};
+  frc::TrapezoidProfile<units::radians> m_profile{
+      {45_deg_per_s, 90_deg_per_s / 1_s}};
 
   frc::TrapezoidProfile<units::radians>::State m_lastProfiledReference;
 
@@ -117,9 +117,7 @@ class Robot : public frc::TimedRobot {
       goal = {kLoweredPosition, 0_rad_per_s};
     }
     m_lastProfiledReference =
-        (frc::TrapezoidProfile<units::radians>(m_constraints, goal,
-                                               m_lastProfiledReference))
-            .Calculate(20_ms);
+        m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
 
     m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
                                     m_lastProfiledReference.velocity.value()});

wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp

@@ -86,8 +86,7 @@ class Robot : public frc::TimedRobot {
   frc::PWMSparkMax m_motor{kMotorPort};
   frc::XboxController m_joystick{kJoystickPort};
 
-  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{3_fps,
-                                                                  6_fps_sq};
+  frc::TrapezoidProfile<units::meters> m_profile{{3_fps, 6_fps_sq}};
 
   frc::TrapezoidProfile<units::meters>::State m_lastProfiledReference;
 
@@ -118,9 +117,7 @@ class Robot : public frc::TimedRobot {
       goal = {kLoweredPosition, 0_fps};
     }
     m_lastProfiledReference =
-        (frc::TrapezoidProfile<units::meters>(m_constraints, goal,
-                                              m_lastProfiledReference))
-            .Calculate(20_ms);
+        m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
 
     m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
                                     m_lastProfiledReference.velocity.value()});

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java

@@ -17,13 +17,13 @@ public ReplaceMeTrapezoidProfileCommand() {
         // The motion profile to be executed
         new TrapezoidProfile(
             // The motion profile constraints
-            new TrapezoidProfile.Constraints(0, 0),
-            // Goal state
-            new TrapezoidProfile.State(),
-            // Initial state
-            new TrapezoidProfile.State()),
+            new TrapezoidProfile.Constraints(0, 0)),
         state -> {
           // Use current trajectory state here
-        });
+        },
+        // Goal state
+        () -> new TrapezoidProfile.State(),
+        // Current state
+        () -> new TrapezoidProfile.State());
   }
 }