diff --git a/commands2/__init__.py b/commands2/__init__.py
index 4d9c2e8c..bb18e552 100644
--- a/commands2/__init__.py
+++ b/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",
diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py
new file mode 100644
index 00000000..922b6a7f
--- /dev/null
+++ b/commands2/ramsetecommand.py
@@ -0,0 +1,229 @@
+# 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, Tuple
+from typing_extensions import overload
+from overtake import overtake
+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 wpimath import units as units
+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.
+ """
+
+ @overload
+ def __init__(
+ self,
+ trajectory: Trajectory,
+ pose: Callable[[], Pose2d],
+ controller: RamseteController,
+ kinematics: DifferentialDriveKinematics,
+ requirements: Tuple[Subsystem],
+ output: Callable[[float, float], 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 requirements: The subsystems to require.
+ :param output A function that consumes the computed left and right outputs in `units.meters_per_second`
+ """
+ super().__init__()
+
+ self._timer = Timer()
+ self._trajectory = trajectory
+ self._pose = pose
+ self._follower = controller
+ self._kinematics = kinematics
+ self._output = output
+ self._usePID = False
+ # All the parameter checks pass, inform scheduler. Type ignoring is set explictitly for the linter because this
+ # implementation declares the tuple explicitly, whereas the general implementations use the unpack operator (*)
+ # to pass the requirements to the scheduler.
+ self.addRequirements(requirements) # type: ignore
+
+ @overload
+ def __init__(
+ self,
+ trajectory: Trajectory,
+ pose: Callable[[], Pose2d],
+ controller: RamseteController,
+ kinematics: DifferentialDriveKinematics,
+ requirements: Tuple[Subsystem],
+ output: Callable[[float, float], None],
+ feedforward: SimpleMotorFeedforwardMeters,
+ leftController: PIDController,
+ rightController: PIDController,
+ wheelSpeeds: Callable[[], DifferentialDriveWheelSpeeds],
+ ):
+ """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 requirements: The subsystems to require.
+ :param output A function that consumes the computed left and right outputs in `units.volts`
+ :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 = output
+ self._leftController = leftController
+ self._rightController = rightController
+ self._wheelspeeds = wheelSpeeds
+ self._feedforward = feedforward
+ self._prevSpeeds = DifferentialDriveWheelSpeeds()
+ self._prevTime = -1.0
+ self._usePID = True
+ # All the parameter checks pass, inform scheduler. Type ignoring is set explictitly for the linter because this
+ # implementation declares the tuple explicitly, whereas the general implementations use the unpack operator (*)
+ # to pass the requirements to the scheduler.
+ self.addRequirements(requirements) # type: ignore
+
+ @overtake(runtime_type_checker="beartype")
+ def __init__(
+ self,
+ *,
+ trajectory: Trajectory,
+ pose: Callable[[], Pose2d],
+ controller: RamseteController,
+ kinematics: DifferentialDriveKinematics,
+ requirements: Tuple[Subsystem],
+ output: Callable[[float, float], None],
+ feedforward: Optional[SimpleMotorFeedforwardMeters] = None,
+ leftController: Optional[PIDController] = None,
+ rightController: Optional[PIDController] = None,
+ wheelSpeeds: Optional[Callable[[], DifferentialDriveWheelSpeeds]] = None,
+ ):
+ ...
+
+ 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
+ )
+ self._output(leftOutput, rightOutput)
+ 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
+ )
diff --git a/setup.py b/setup.py
index b1c069cb..99b6e032 100644
--- a/setup.py
+++ b/setup.py
@@ -11,7 +11,12 @@
description="WPILib command framework v2",
url="https://github.com/robotpy/robotpy-commands-v2",
packages=["commands2"],
- install_requires=["wpilib<2025,>=2024.0.0b2", "typing_extensions>=4.1.0,<5"],
+ install_requires=[
+ "wpilib<2025,>=2024.0.0b2",
+ "typing_extensions>=4.1.0,<5",
+ "overtake~=0.4.0",
+ "beartype~=0.16.4",
+ ],
license="BSD-3-Clause",
python_requires=">=3.8",
include_package_data=True,
diff --git a/tests/test_ramsetecommand.py b/tests/test_ramsetecommand.py
new file mode 100644
index 00000000..604380d5
--- /dev/null
+++ b/tests/test_ramsetecommand.py
@@ -0,0 +1,424 @@
+# 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
+import wpimath.units as units
+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 = 6.0 / 12.0
+ self.kyTolerance = 6.0 / 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 setWheelSpeedsMPS(
+ self, leftspeed: units.meters_per_second, rightspeed: units.meters_per_second
+ ) -> None:
+ self.leftSpeed = leftspeed
+ self.rightSpeed = rightspeed
+
+ def setWheelSpeedsVolts(
+ self, leftVolts: units.volts, rightVolts: units.volts
+ ) -> None:
+ self.leftSpeed = leftVolts
+ self.rightSpeed = rightVolts
+
+ 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()
+
+ def getWheelSpeeds(self) -> kinematics.DifferentialDriveWheelSpeeds:
+ return kinematics.DifferentialDriveWheelSpeeds(self.leftSpeed, self.rightSpeed)
+
+
+@pytest.fixture()
+def get_ramsete_command_data() -> RamseteCommandTestDataFixtures:
+ return RamseteCommandTestDataFixtures()
+
+
+def test_rameseteRaisesNoOutputRaises(
+ 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())
+
+ with pytest.raises(Exception):
+ command = commands2.RamseteCommand(
+ new_trajectory,
+ fixture_data.getRobotPose,
+ controller.RamseteController(
+ fixture_data.kRamseteB, fixture_data.kRamseteZeta
+ ),
+ fixture_data.command_kinematics,
+ subsystem,
+ )
+
+
+def test_rameseteRaisesOnlyFeedForward(
+ 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())
+ feedforward_var: controller.SimpleMotorFeedforwardMeters = (
+ controller.SimpleMotorFeedforwardMeters(
+ fixture_data.ksVolts,
+ fixture_data.kvVoltSecondsPerMeter,
+ fixture_data.kaVoltSecondsSquaredPerMeter,
+ )
+ )
+
+ with pytest.raises(Exception):
+ command = commands2.RamseteCommand(
+ new_trajectory,
+ fixture_data.getRobotPose,
+ controller.RamseteController(
+ fixture_data.kRamseteB, fixture_data.kRamseteZeta
+ ),
+ fixture_data.command_kinematics,
+ subsystem,
+ fixture_data.setWheelSpeedsMPS,
+ feedforward_var,
+ )
+
+
+def test_rameseteRaisesFeedForwardAndLeft(
+ 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())
+ feedforward_var: controller.SimpleMotorFeedforwardMeters = (
+ controller.SimpleMotorFeedforwardMeters(
+ fixture_data.ksVolts,
+ fixture_data.kvVoltSecondsPerMeter,
+ fixture_data.kaVoltSecondsSquaredPerMeter,
+ )
+ )
+ left_pid: controller.PIDController = controller.PIDController(0.1, 0, 0)
+
+ with pytest.raises(Exception):
+ command = commands2.RamseteCommand(
+ new_trajectory,
+ fixture_data.getRobotPose,
+ controller.RamseteController(
+ fixture_data.kRamseteB, fixture_data.kRamseteZeta
+ ),
+ fixture_data.command_kinematics,
+ subsystem,
+ fixture_data.setWheelSpeedsMPS,
+ feedforward_var,
+ left_pid,
+ )
+
+
+def test_rameseteRaisesFeedForwardRightAndLeft(
+ 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())
+ feedforward_var: controller.SimpleMotorFeedforwardMeters = (
+ controller.SimpleMotorFeedforwardMeters(
+ fixture_data.ksVolts,
+ fixture_data.kvVoltSecondsPerMeter,
+ fixture_data.kaVoltSecondsSquaredPerMeter,
+ )
+ )
+ left_pid: controller.PIDController = controller.PIDController(0.1, 0, 0)
+ right_pid: controller.PIDController = controller.PIDController(0.1, 0, 0)
+
+ with pytest.raises(Exception):
+ command = commands2.RamseteCommand(
+ new_trajectory,
+ fixture_data.getRobotPose,
+ controller.RamseteController(
+ fixture_data.kRamseteB, fixture_data.kRamseteZeta
+ ),
+ fixture_data.command_kinematics,
+ subsystem,
+ fixture_data.setWheelSpeedsMPS,
+ feedforward_var,
+ left_pid,
+ right_pid,
+ )
+
+
+def test_ramseteCommandMPSReachesDestination(
+ 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,
+ subsystem,
+ fixture_data.setWheelSpeedsMPS,
+ )
+
+ 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
+ )
+
+
+def test_ramseteCommandVoltsReachesDestination(
+ 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,
+ subsystem,
+ fixture_data.setWheelSpeedsVolts,
+ )
+
+ 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
+ )
+
+
+def test_ramseteCommandPIDReachesDestination(
+ 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())
+ feedforward_var: controller.SimpleMotorFeedforwardMeters = (
+ controller.SimpleMotorFeedforwardMeters(
+ fixture_data.ksVolts,
+ fixture_data.kvVoltSecondsPerMeter,
+ fixture_data.kaVoltSecondsSquaredPerMeter,
+ )
+ )
+ left_pid: controller.PIDController = controller.PIDController(0.001, 0, 0)
+ rightt_pid: controller.PIDController = controller.PIDController(0.001, 0, 0)
+
+ command = commands2.RamseteCommand(
+ new_trajectory,
+ fixture_data.getRobotPose,
+ controller.RamseteController(
+ fixture_data.kRamseteB, fixture_data.kRamseteZeta
+ ),
+ fixture_data.command_kinematics,
+ subsystem,
+ fixture_data.setWheelSpeedsVolts,
+ feedforward_var,
+ left_pid,
+ rightt_pid,
+ fixture_data.getWheelSpeeds,
+ )
+
+ 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
+ )