Added RamseteCommand to Commands 2.

commands2/__init__.py

@@ -35,6 +35,7 @@
 from .printcommand import PrintCommand
 from .proxycommand import ProxyCommand
 from .proxyschedulecommand import ProxyScheduleCommand
+from .ramsetecommand import RamseteCommand
 from .repeatcommand import RepeatCommand
 from .runcommand import RunCommand
 from .schedulecommand import ScheduleCommand
@@ -68,6 +69,7 @@
     "PrintCommand",
     "ProxyCommand",
     "ProxyScheduleCommand",
+    "RamseteCommand",
     "RepeatCommand",
     "RunCommand",
     "ScheduleCommand",

commands2/ramsetecommand.py

@@ -0,0 +1,185 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+from __future__ import annotations
+
+from typing import Callable, Union, Optional
+from wpimath.controller import (
+    PIDController,
+    RamseteController,
+    SimpleMotorFeedforwardMeters,
+)
+from wpimath.geometry import Pose2d
+from wpimath.kinematics import (
+    ChassisSpeeds,
+    DifferentialDriveKinematics,
+    DifferentialDriveWheelSpeeds,
+)
+from wpimath.trajectory import Trajectory
+from wpiutil import SendableBuilder
+from wpilib import Timer
+
+
+from .command import Command
+from .subsystem import Subsystem
+
+
+class RamseteCommand(Command):
+    """
+    A command that uses a RAMSETE controller (RamseteController) to follow a trajectory
+    (Trajectory) with a differential drive.
+
+    The command handles trajectory-following, PID calculations, and feedforwards internally. This
+    is intended to be a more-or-less "complete solution" that can be used by teams without a great
+    deal of controls expertise.
+
+    Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
+    functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
+    and feedforward functionality, returning only the raw wheel speeds from the RAMSETE controller.
+
+    This class is provided by the NewCommands VendorDep
+    """
+
+    def __init__(
+        self,
+        trajectory: Trajectory,
+        pose: Callable[[], Pose2d],
+        controller: RamseteController,
+        kinematics: DifferentialDriveKinematics,
+        outputVolts: Callable[[float, float], None],
+        *requirements: Subsystem,
+        feedforward: Optional[SimpleMotorFeedforwardMeters] = None,
+        leftController: Optional[PIDController] = None,
+        rightController: Optional[PIDController] = None,
+        wheelSpeeds: Optional[Callable[[], DifferentialDriveWheelSpeeds]] = None,
+    ):
+        """Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID
+        control and feedforward are handled internally, and outputs are scaled -12 to 12 representing
+        units of volts.
+
+        Note: The controller 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 controller:      The RAMSETE controller used to follow the trajectory.
+        :param kinematics:      The kinematics for the robot drivetrain.
+        :param outputVolts:     A function that consumes the computed left and right outputs (in volts) for
+                                the robot drive.
+        :param requirements:    The subsystems to require.
+
+        OPTIONAL PARAMETERS
+        When the following optional parameters are provided, when executed, the RamseteCommand will follow
+        provided trajectory. PID control and feedforward are handled internally, and the outputs are scaled
+        from -12 to 12 representing units of volts. If any one of the optional parameters are provided, each
+        other optional parameter becomes required.
+        :param feedforward      The feedforward to use for the drive
+        :param leftController:  The PIDController for the left side of the robot drive.
+        :param rightController: The PIDController for the right side of the robot drive.
+        :param wheelSpeeds:     A function that supplies the speeds of the left and right sides of the robot
+                                drive.
+        """
+        super().__init__()
+
+        self._timer = Timer()
+
+        # Store all the requirements that are required
+        self.trajectory = trajectory
+        self.pose = pose
+        self.follower = controller
+        self.kinematics = kinematics
+        self.output = outputVolts
+        self.usePID = False
+        # Optional requirements checks. If any one of the optional parameters are not None, then all the optional
+        # requirements become required.
+        if feedforward or leftController or rightController or wheelSpeeds is not None:
+            if feedforward or leftController or rightController or wheelSpeeds is None:
+                raise RuntimeError(
+                    f"Failed to instantiate RamseteCommand, not all optional arguments were provided.\n \
+                                    Feedforward - {feedforward}, LeftController - {leftController}, RightController - {rightController}, WheelSpeeds - {wheelSpeeds} "
+                )
+
+            self.leftController = leftController
+            self.rightController = rightController
+            self.wheelspeeds = wheelSpeeds
+            self.feedforward = feedforward
+            self.usePID = True
+        self._prevSpeeds = DifferentialDriveWheelSpeeds()
+        self._prevTime = -1.0
+
+        self.addRequirements(*requirements)
+
+    def initialize(self):
+        self._prevTime = -1
+        initial_state = self.trajectory.sample(0)
+        self._prevSpeeds = self.kinematics.toWheelSpeeds(
+            ChassisSpeeds(
+                initial_state.velocity,
+                0,
+                initial_state.curvature * initial_state.velocity,
+            )
+        )
+        self._timer.restart()
+        if self.usePID:
+            self.leftController.reset()
+            self.rightController.reset()
+
+    def execute(self):
+        curTime = self._timer.get()
+        dt = curTime - self._prevTime
+
+        if self._prevTime < 0:
+            self.output(0.0, 0.0)
+            self._prevTime = curTime
+            return
+
+        targetWheelSpeeds = self.kinematics.toWheelSpeeds(
+            self.follower.calculate(self.pose(), self.trajectory.sample(curTime))
+        )
+
+        leftSpeedSetpoint = targetWheelSpeeds.left
+        rightSpeedSetpoint = targetWheelSpeeds.right
+
+        if self.usePID:
+            leftFeedforward = self.feedforward.calculate(
+                leftSpeedSetpoint, (leftSpeedSetpoint - self._prevSpeeds.left) / dt
+            )
+
+            rightFeedforward = self.feedforward.calculate(
+                rightSpeedSetpoint,
+                (rightSpeedSetpoint - self._prevSpeeds.right) / dt,
+            )
+
+            leftOutput = leftFeedforward + self.leftController.calculate(
+                self.wheelspeeds().left, leftSpeedSetpoint
+            )
+
+            rightOutput = rightFeedforward + self.rightController.calculate(
+                self.wheelspeeds().right, rightSpeedSetpoint
+            )
+        else:
+            leftOutput = leftSpeedSetpoint
+            rightOutput = rightSpeedSetpoint
+
+        self.output(leftOutput, rightOutput)
+        self._prevSpeeds = targetWheelSpeeds
+        self._prevTime = curTime
+
+    def end(self, interrupted: bool):
+        self._timer.stop()
+
+        if interrupted:
+            self.output(0.0, 0.0)
+
+    def isFinished(self):
+        return self._timer.hasElapsed(self.trajectory.totalTime())
+
+    def initSendable(self, builder: SendableBuilder):
+        super().initSendable(builder)
+        builder.addDoubleProperty(
+            "leftVelocity", lambda: self._prevSpeeds.left, lambda *float: None
+        )
+        builder.addDoubleProperty(
+            "rightVelocity", lambda: self._prevSpeeds.right, lambda *float: None
+        )

tests/test_ramsetecommand.py

@@ -0,0 +1,148 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+
+from typing import TYPE_CHECKING, List
+import math
+
+import wpimath.controller as controller
+import wpimath.trajectory as trajectory
+import wpimath.trajectory.constraint as constraints
+import wpimath.geometry as geometry
+import wpimath.kinematics as kinematics
+from wpilib import Timer
+
+from util import *  # type: ignore
+
+if TYPE_CHECKING:
+    from .util import *
+
+import pytest
+
+import commands2
+
+
+class RamseteCommandTestDataFixtures:
+    def __init__(self):
+        self.timer = Timer()
+        self.angle: geometry.Rotation2d = geometry.Rotation2d(0)
+
+        # Track speeds and distances
+        self.leftSpeed = 0
+        self.leftDistance = 0
+        self.rightSpeed = 0
+        self.rightDistance = 0
+
+        # Chassis/Drivetrain constants
+        self.kxTolerance = 1 / 12.0
+        self.kyTolerance = 1 / 12.0
+        self.kWheelBase = 0.5
+        self.kTrackWidth = 0.5
+        self.kWheelDiameterMeters = 0.1524
+        self.kRamseteB = 2.0
+        self.kRamseteZeta = 0.7
+        self.ksVolts = 0.22
+        self.kvVoltSecondsPerMeter = 1.98
+        self.kaVoltSecondsSquaredPerMeter = 0.2
+        self.kPDriveVel = 8.5
+        self.kMaxMetersPerSecond = 3.0
+        self.kMaxAccelerationMetersPerSecondSquared = 1.0
+        self.kEncoderCPR = 1024
+        self.kEncoderDistancePerPulse = (
+            self.kWheelDiameterMeters * math.pi
+        ) / self.kEncoderCPR
+
+        self.command_kinematics: kinematics.DifferentialDriveKinematics = (
+            kinematics.DifferentialDriveKinematics(self.kTrackWidth)
+        )
+
+        self.command_voltage_constraint: constraints.DifferentialDriveVoltageConstraint = constraints.DifferentialDriveVoltageConstraint(
+            controller.SimpleMotorFeedforwardMeters(
+                self.ksVolts,
+                self.kvVoltSecondsPerMeter,
+                self.kaVoltSecondsSquaredPerMeter,
+            ),
+            self.command_kinematics,
+            10,
+        )
+
+        self.command_odometry: kinematics.DifferentialDriveOdometry = (
+            kinematics.DifferentialDriveOdometry(
+                self.angle,
+                self.leftDistance,
+                self.rightDistance,
+                geometry.Pose2d(0, 0, geometry.Rotation2d(0)),
+            )
+        )
+
+    def setWheelSpeeds(self, leftspeed: float, rightspeed: float) -> None:
+        self.leftSpeed = leftspeed
+        self.rightSpeed = rightspeed
+
+    def getCurrentWheelDistances(self) -> kinematics.DifferentialDriveWheelPositions:
+        positions = kinematics.DifferentialDriveWheelPositions()
+        positions.right = self.rightDistance
+        positions.left = self.leftDistance
+
+        return positions
+
+    def getRobotPose(self) -> geometry.Pose2d:
+        positions = self.getCurrentWheelDistances()
+        self.command_odometry.update(self.angle, positions.left, positions.right)
+        return self.command_odometry.getPose()
+
+
+@pytest.fixture()
+def get_ramsete_command_data() -> RamseteCommandTestDataFixtures:
+    return RamseteCommandTestDataFixtures()
+
+
+def test_ramseteCommand(
+    scheduler: commands2.CommandScheduler, get_ramsete_command_data
+):
+    with ManualSimTime() as sim:
+        fixture_data = get_ramsete_command_data
+        subsystem = commands2.Subsystem()
+        waypoints: List[geometry.Pose2d] = []
+        waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0)))
+        waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0)))
+        traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1)
+        traj_config.setKinematics(fixture_data.command_kinematics)
+        traj_config.addConstraint(fixture_data.command_voltage_constraint)
+        new_trajectory: trajectory.Trajectory = (
+            trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config)
+        )
+        end_state = new_trajectory.sample(new_trajectory.totalTime())
+
+        command = commands2.RamseteCommand(
+            new_trajectory,
+            fixture_data.getRobotPose,
+            controller.RamseteController(
+                fixture_data.kRamseteB, fixture_data.kRamseteZeta
+            ),
+            fixture_data.command_kinematics,
+            fixture_data.setWheelSpeeds,
+            subsystem,
+        )
+
+        fixture_data.timer.restart()
+
+        command.initialize()
+
+        while not command.isFinished():
+            command.execute()
+
+            fixture_data.leftDistance += fixture_data.leftSpeed * 0.005
+            fixture_data.rightDistance += fixture_data.rightSpeed * 0.005
+
+            sim.step(0.005)
+
+        fixture_data.timer.stop()
+        command.end(True)
+
+        assert end_state.pose.X() == pytest.approx(
+            fixture_data.getRobotPose().X(), fixture_data.kxTolerance
+        )
+        assert end_state.pose.Y() == pytest.approx(
+            fixture_data.getRobotPose().Y(), fixture_data.kyTolerance
+        )