From fe6167dc0c71cdb052e2a4c0c42050ee85b5ad78 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 28 Dec 2023 12:51:18 -0500 Subject: [PATCH] Updated implementation to use overload typing constructors and tests to verify proper instantiation and functionality. --- commands2/ramsetecommand.py | 102 +++++++++-- tests/test_ramsetecommand.py | 316 ++++++++++++++++++++++++++++++++++- 2 files changed, 400 insertions(+), 18 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index be9c839c..3043bbf9 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -3,7 +3,7 @@ # the WPILib BSD license file in the root directory of this project. from __future__ import annotations -from typing import Callable, Union, Optional +from typing import Callable, Union, Optional, Tuple, overload from wpimath.controller import ( PIDController, RamseteController, @@ -16,6 +16,7 @@ DifferentialDriveWheelSpeeds, ) from wpimath.trajectory import Trajectory +from wpimath import units as units from wpiutil import SendableBuilder from wpilib import Timer @@ -36,18 +37,35 @@ class RamseteCommand(Command): 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 """ + @overload + 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] + ], + ): + ... + def __init__( self, trajectory: Trajectory, pose: Callable[[], Pose2d], controller: RamseteController, kinematics: DifferentialDriveKinematics, - outputVolts: Callable[[float, float], None], - *requirements: Subsystem, + requirements: Tuple[Subsystem], + *, + outputVolts: Optional[Callable[[units.volts, units.volts], None]] = None, + outputMPS: Optional[ + Callable[[units.meters_per_second, units.meters_per_second], None] + ] = None, feedforward: Optional[SimpleMotorFeedforwardMeters] = None, leftController: Optional[PIDController] = None, rightController: Optional[PIDController] = None, @@ -65,10 +83,39 @@ def __init__( 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. + 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 @@ -89,12 +136,38 @@ def __init__( self.pose = pose self.follower = controller self.kinematics = kinematics - self.output = outputVolts - self.usePID = False + + # 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." + ) + + # If the above check fails, then one of them is not None. Take the one supplied and assign it for output. + if outputMPS is not None: + self.output = outputMPS + else: + self.output = outputVolts + # 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: + 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} " @@ -105,10 +178,12 @@ def __init__( self.wheelspeeds = wheelSpeeds self.feedforward = feedforward self.usePID = True + else: + self.usePID = False self._prevSpeeds = DifferentialDriveWheelSpeeds() self._prevTime = -1.0 - self.addRequirements(*requirements) + self.addRequirements(requirements) def initialize(self): self._prevTime = -1 @@ -158,11 +233,12 @@ def execute(self): rightOutput = rightFeedforward + self.rightController.calculate( self.wheelspeeds().right, rightSpeedSetpoint ) + self.output(leftOutput, rightOutput) else: leftOutput = leftSpeedSetpoint rightOutput = rightSpeedSetpoint + self.output(leftOutput, rightOutput) - self.output(leftOutput, rightOutput) self._prevSpeeds = targetWheelSpeeds self._prevTime = curTime diff --git a/tests/test_ramsetecommand.py b/tests/test_ramsetecommand.py index 70e76511..e383781b 100644 --- a/tests/test_ramsetecommand.py +++ b/tests/test_ramsetecommand.py @@ -10,6 +10,7 @@ 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 @@ -34,8 +35,8 @@ def __init__(self): self.rightDistance = 0 # Chassis/Drivetrain constants - self.kxTolerance = 1 / 12.0 - self.kyTolerance = 1 / 12.0 + self.kxTolerance = 6.0 / 12.0 + self.kyTolerance = 6.0 / 12.0 self.kWheelBase = 0.5 self.kTrackWidth = 0.5 self.kWheelDiameterMeters = 0.1524 @@ -75,10 +76,18 @@ def __init__(self): ) ) - def setWheelSpeeds(self, leftspeed: float, rightspeed: float) -> None: + 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 @@ -91,13 +100,247 @@ def getRobotPose(self) -> geometry.Pose2d: 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_ramseteCommand( +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: @@ -121,8 +364,71 @@ def test_ramseteCommand( fixture_data.kRamseteB, fixture_data.kRamseteZeta ), fixture_data.command_kinematics, - fixture_data.setWheelSpeeds, 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()