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
+ )