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..3c3a4b10 --- /dev/null +++ b/commands2/ramsetecommand.py @@ -0,0 +1,280 @@ +# 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, overload +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], + *, + outputMPS: Callable[[units.meters_per_second, units.meters_per_second], None], + ): + ... + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + controller: RamseteController, + kinematics: DifferentialDriveKinematics, + requirements: Tuple[Subsystem], + *, + outputVolts: Callable[[units.volts, units.volts], None], + feedforward: SimpleMotorFeedforwardMeters, + leftController: PIDController, + rightController: PIDController, + wheelSpeeds: Callable[[], DifferentialDriveWheelSpeeds], + ): + ... + + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + controller: RamseteController, + kinematics: DifferentialDriveKinematics, + requirements: Tuple[Subsystem], + *, + outputMPS: Optional[ + Callable[[units.meters_per_second, units.meters_per_second], None] + ] = None, + outputVolts: Optional[Callable[[units.volts, units.volts], None]] = None, + 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 requirements: The subsystems to require. + + REQUIRED KEYWORD PARAMETERS + Note: The output mode must be specified by the users of RamseteCommands. Users can specify ONE of + an output function that will supply the consumer with left and right speeds in `units.meters_per_second` + or in `units.volts`. If neither, or both, are supplied to the constructor a `RuntimeError` will be + raised. + :param outputVolts A function that consumes the computed left and right outputs in `units.volts` + :param outputMPS A function that consumes the computed left and right outputs in `units.meters_per_second` + + + 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. + + REQUIRED KEYWORD PARAMETERS + Note: The output mode must be specified by the users of RamseteCommands. Users can specify ONE of + an output function that will supply the consumer with left and right speeds in `units.meters_per_second` + or in `units.volts`. If neither, or both, are supplied to the constructor a `RuntimeError` will be + raised. + :param outputVolts A function that consumes the computed left and right outputs in `units.volts` + :param outputMPS A function that consumes the computed left and right outputs in `units.meters_per_second` + + 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.outputMPS = outputMPS + self.outputVolts = outputVolts + self.leftController = leftController + self.rightController = rightController + self.wheelspeeds = wheelSpeeds + self.feedforward = feedforward + self._prevSpeeds = DifferentialDriveWheelSpeeds() + self._prevTime = -1.0 + + # Required requirements check for output + if outputMPS is None and outputVolts is None: + raise RuntimeError( + f"Failed to instantiate RamseteCommand, no output consumer provided. Must provide either output in meters per second or volts." + ) + + if outputMPS is not None and outputVolts is not None: + raise RuntimeError( + f"Failed to instantiate RamseteCommand, too many consumers provided. Must provide either output in meters per second or volts but not both." + ) + + # Optional requirements checks. If any one of the optional parameters are not None, then all the optional + # requirements become required. + if ( + feedforward is not None + or leftController is not None + or rightController is not None + or wheelSpeeds is not None + ): + if ( + feedforward is None + or leftController is None + or rightController is None + 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.usePID = True + else: + 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 + + 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: + if self.outputVolts is not None: + self.outputVolts(0.0, 0.0) + if self.outputMPS is not None: + self.outputMPS(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.outputVolts(leftOutput, rightOutput) + else: + leftOutput = leftSpeedSetpoint + rightOutput = rightSpeedSetpoint + self.outputMPS(leftOutput, rightOutput) + + self._prevSpeeds = targetWheelSpeeds + self._prevTime = curTime + + def end(self, interrupted: bool): + self._timer.stop() + + if interrupted: + if self.outputVolts is not None: + self.outputVolts(0.0, 0.0) + if self.outputMPS is not None: + self.outputMPS(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/tests/test_ramsetecommand.py b/tests/test_ramsetecommand.py new file mode 100644 index 00000000..e383781b --- /dev/null +++ b/tests/test_ramsetecommand.py @@ -0,0 +1,454 @@ +# 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.volt_seconds, rightVolts: units.volt_seconds + ) -> 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(RuntimeError): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + ) + + +def test_rameseteRaisesBothOutputRaises( + 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(RuntimeError): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + outputMPS=fixture_data.setWheelSpeedsMPS, + outputVolts=fixture_data.setWheelSpeedsVolts, + ) + + +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(RuntimeError): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + outputMPS=fixture_data.setWheelSpeedsMPS, + feedforward=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(RuntimeError): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + outputMPS=fixture_data.setWheelSpeedsMPS, + feedforward=feedforward_var, + leftController=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) + rightt_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) + + with pytest.raises(RuntimeError): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + outputMPS=fixture_data.setWheelSpeedsMPS, + feedforward=feedforward_var, + leftController=left_pid, + rightController=rightt_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, + outputMPS=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, + outputMPS=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, + outputVolts=fixture_data.setWheelSpeedsVolts, + feedforward=feedforward_var, + leftController=left_pid, + rightController=rightt_pid, + wheelSpeeds=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 + )