Added RamseteCommand to Commands 2. robotpy#28

commands2/__init__.py

@@ -34,6 +34,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
@@ -65,6 +66,7 @@
     "PrintCommand",
     "ProxyCommand",
     "ProxyScheduleCommand",
+    "RamseteCommand",
     "RepeatCommand",
     "RunCommand",
     "ScheduleCommand",

commands2/ramsetecommand.py

@@ -0,0 +1,182 @@
+# 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
+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: SimpleMotorFeedforwardMeters | None = None,
+        leftController: PIDController | None = None,
+        rightController: PIDController | None = None,
+        wheelSpeeds: Callable[[], DifferentialDriveWheelSpeeds] | None = 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 = trajectory
+        self.pose = pose
+        self.follower: RamseteController = controller
+        self.kinematics = kinematics
+        self.output: Callable[[float, float], None] = outputVolts
+        self.leftController = None
+        self.rightController = None
+        self.feedforward = None
+        self.wheelspeeds = None
+        self.usePID = False
+        # Optional requirements checks. If any one of the optionl 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: PIDController = leftController
+            self.rightController: PIDController = rightController
+            self.wheelspeeds: Callable[[], DifferentialDriveWheelSpeeds] = wheelSpeeds
+            self.feedforward: SimpleMotorFeedforwardMeters = 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)
+        initial_speeds = self.kinematics.toWheelSpeeds(
+            ChassisSpeeds( initial_state.velocity, 0, initial_state.curvature * initial_state.velocity )
+        )
+        self.prevSpeeds = initial_speeds
+        self.timer.restart()
+        if self.usePID:
+            self.leftController.reset()
+            self.rightController.reset()
+
+    def execute(self):
+        cur_time = self.timer.get()
+        dt = cur_time - self.prevTime
+
+        if self.prevTime < 0:
+            self.output(0.0, 0.0)
+            self.prevTime = cur_time
+            return
+
+        target_wheel_speeds = self.kinematics.toWheelSpeeds(
+            self.follower.calculate(self.pose(), self.trajectory.sample(cur_time))
+        )
+
+        left_speed_setpoint = target_wheel_speeds.left
+        right_speed_setpoint = target_wheel_speeds.right
+
+        if self.usePID:
+            left_feedforward = self.feedforward.calculate(
+                left_speed_setpoint,
+                (left_speed_setpoint - self.prevSpeeds.left) / dt
+            )
+
+            right_feedforward = self.feedforward.calculate(
+                right_speed_setpoint,
+                (right_speed_setpoint - self.prevSpeeds.right) / dt
+            )
+
+            left_output = (
+                left_feedforward
+                + self.leftController.calculate(
+                    self.wheelspeeds().left,
+                    left_speed_setpoint
+                )
+            )
+
+            right_output = (
+                right_feedforward
+                + self.rightController.calculate(
+                    self.wheelspeeds().right,
+                    right_speed_setpoint
+                )
+            )
+        else:
+            left_output = left_speed_setpoint
+            right_output = right_speed_setpoint
+
+        self.output(left_output, right_output)
+        self.prevSpeeds = target_wheel_speeds
+        self.prevTime = cur_time
+
+    def end(self, interrupted):
+        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, None)
+        builder.addDoubleProperty("rightVelocity", lambda: self.prevSpeeds.right, None)

tests/test_ramsetecommand.py

@@ -0,0 +1,142 @@
+# 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
+        )