[commands] Add constructor for SwerveControllerCommand that takes a HolonomicDriveController (#4785)

Also adds copy and move constructors to HolonomicDriveController.

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

@@ -70,23 +70,17 @@ public class SwerveControllerCommand extends CommandBase {
       Supplier<Rotation2d> desiredRotation,
       Consumer<SwerveModuleState[]> outputModuleStates,
       Subsystem... requirements) {
-    m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
+    this(
-    m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
+        trajectory,
-    m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
+        pose,
-
+        kinematics,
-    m_controller =
         new HolonomicDriveController(
             requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
             requireNonNullParam(yController, "yController", "SwerveControllerCommand"),
-            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
+            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")),
-
+        desiredRotation,
-    m_outputModuleStates =
+        outputModuleStates,
-        requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand");
+        requirements);
-
-    m_desiredRotation =
-        requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
-
-    addRequirements(requirements);
   }
 
   /**
@@ -134,6 +128,85 @@ public class SwerveControllerCommand extends CommandBase {
         requirements);
   }
 
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the provided
+   * trajectory. This command will not return output voltages but rather raw module states from the
+   * position controllers which need to be put into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
+   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
+   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
+   * should be used.
+   *
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param controller The HolonomicDriveController for the drivetrain.
+   * @param outputModuleStates The raw output module states from the position controllers.
+   * @param requirements The subsystems to require.
+   */
+  public SwerveControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SwerveDriveKinematics kinematics,
+      HolonomicDriveController controller,
+      Consumer<SwerveModuleState[]> outputModuleStates,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        kinematics,
+        controller,
+        () ->
+            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
+        outputModuleStates,
+        requirements);
+  }
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the provided
+   * trajectory. This command will not return output voltages but rather raw module states from the
+   * position controllers which need to be put into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param controller The HolonomicDriveController for the drivetrain.
+   * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each
+   *     time step.
+   * @param outputModuleStates The raw output module states from the position controllers.
+   * @param requirements The subsystems to require.
+   */
+  public SwerveControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SwerveDriveKinematics kinematics,
+      HolonomicDriveController controller,
+      Supplier<Rotation2d> desiredRotation,
+      Consumer<SwerveModuleState[]> outputModuleStates,
+      Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
+    m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
+    m_controller = requireNonNullParam(controller, "controller", "SwerveControllerCommand");
+
+    m_desiredRotation =
+        requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
+
+    m_outputModuleStates =
+        requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand");
+
+    addRequirements(requirements);
+  }
+
   @Override
   public void initialize() {
     m_timer.reset();

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

@@ -209,6 +209,131 @@ class SwerveControllerCommand
           output,
       std::span<Subsystem* const> requirements = {});
 
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param controller      The HolonomicDriveController for the drivetrain.
+   * @param desiredRotation The angle that the drivetrain should be
+   *                        facing. This is sampled at each time step.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc::HolonomicDriveController controller,
+      std::function<frc::Rotation2d()> desiredRotation,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param controller      The HolonomicDriveController for the drivetrain.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc::HolonomicDriveController controller,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      std::initializer_list<Subsystem*> requirements);
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param controller     The HolonomicDriveController for the robot.
+   * @param desiredRotation The angle that the drivetrain should be
+   *                        facing. This is sampled at each time step.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc::HolonomicDriveController controller,
+      std::function<frc::Rotation2d()> desiredRotation,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      std::span<Subsystem* const> requirements = {});
+
+  /**
+   * Constructs a new SwerveControllerCommand that when executed will follow the
+   * provided trajectory. This command will not return output voltages but
+   * rather raw module states from the position controllers which need to be put
+   * into a velocity PID.
+   *
+   * <p>Note: The controllers will *not* set the outputVolts to zero upon
+   * completion of the path- this is left to the user, since it is not
+   * appropriate for paths with nonstationary endstates.
+   *
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of
+   * the final pose in the trajectory. The robot will not follow the rotations
+   * from the poses at each timestep. If alternate rotation behavior is desired,
+   * the other constructor with a supplier for rotation should be used.
+   *
+   * @param trajectory      The trajectory to follow.
+   * @param pose            A function that supplies the robot pose,
+   *                        provided by the odometry class.
+   * @param kinematics      The kinematics for the robot drivetrain.
+   * @param controller      The HolonomicDriveController for the drivetrain.
+   * @param output          The raw output module states from the
+   *                        position controllers.
+   * @param requirements    The subsystems to require.
+   */
+  SwerveControllerCommand(
+      frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      frc::HolonomicDriveController controller,
+      std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
+          output,
+      std::span<Subsystem* const> requirements = {});
+
   void Initialize() override;
 
   void Execute() override;

wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc

@@ -79,6 +79,70 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
   this->AddRequirements(requirements);
 }
 
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc::HolonomicDriveController controller,
+    std::function<frc::Rotation2d()> desiredRotation,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
+      m_kinematics(kinematics),
+      m_controller(std::move(controller)),
+      m_desiredRotation(std::move(desiredRotation)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc::HolonomicDriveController controller,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    std::initializer_list<Subsystem*> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
+      m_kinematics(kinematics),
+      m_controller(std::move(controller)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc::HolonomicDriveController controller,
+    std::function<frc::Rotation2d()> desiredRotation,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    std::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
+      m_kinematics(kinematics),
+      m_controller(std::move(controller)),
+      m_desiredRotation(std::move(desiredRotation)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
+template <size_t NumModules>
+SwerveControllerCommand<NumModules>::SwerveControllerCommand(
+    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    frc::HolonomicDriveController controller,
+    std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
+    std::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
+      m_kinematics(kinematics),
+      m_controller(std::move(controller)),
+      m_outputStates(output) {
+  this->AddRequirements(requirements);
+}
+
 template <size_t NumModules>
 void SwerveControllerCommand<NumModules>::Initialize() {
   if (m_desiredRotation == nullptr) {

wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h

@@ -45,6 +45,12 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
       frc2::PIDController xController, frc2::PIDController yController,
       ProfiledPIDController<units::radian> thetaController);
 
+  HolonomicDriveController(const HolonomicDriveController&) = default;
+  HolonomicDriveController& operator=(const HolonomicDriveController&) =
+      default;
+  HolonomicDriveController(HolonomicDriveController&&) = default;
+  HolonomicDriveController& operator=(HolonomicDriveController&&) = default;
+
   /**
    * Returns true if the pose error is within tolerance of the reference.
    */