From 0faf2c0ca64d7b272678ec745f3ad00271c7917c Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Wed, 6 Dec 2023 15:13:36 -0500 Subject: [PATCH 01/10] Added RamseteCommand to Commands 2. robotpy/robotpy-commands-v2#28 --- commands2/__init__.py | 2 + commands2/ramsetecommand.py | 182 +++++++++++++++++++++++++++++++++++ tests/test_ramsetecommand.py | 142 +++++++++++++++++++++++++++ 3 files changed, 326 insertions(+) create mode 100644 commands2/ramsetecommand.py create mode 100644 tests/test_ramsetecommand.py diff --git a/commands2/__init__.py b/commands2/__init__.py index cc8e6a4c..719d5437 100644 --- a/commands2/__init__.py +++ b/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", diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py new file mode 100644 index 00000000..07146195 --- /dev/null +++ b/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) diff --git a/tests/test_ramsetecommand.py b/tests/test_ramsetecommand.py new file mode 100644 index 00000000..ee6b69af --- /dev/null +++ b/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 + ) \ No newline at end of file From ce75329cef105948cf6120074e0dd15aa7f5dbb3 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Wed, 6 Dec 2023 15:16:17 -0500 Subject: [PATCH 02/10] Fixed formatting with Black. --- commands2/ramsetecommand.py | 50 +++++++++++++++++++----------------- tests/test_ramsetecommand.py | 38 +++++++++++++++------------ 2 files changed, 49 insertions(+), 39 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index 07146195..7d378ef7 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -4,9 +4,17 @@ from __future__ import annotations from typing import Callable, Union -from wpimath.controller import PIDController, RamseteController, SimpleMotorFeedforwardMeters +from wpimath.controller import ( + PIDController, + RamseteController, + SimpleMotorFeedforwardMeters, +) from wpimath.geometry import Pose2d -from wpimath.kinematics import ChassisSpeeds, DifferentialDriveKinematics, DifferentialDriveWheelSpeeds +from wpimath.kinematics import ( + ChassisSpeeds, + DifferentialDriveKinematics, + DifferentialDriveWheelSpeeds, +) from wpimath.trajectory import Trajectory from wpiutil import SendableBuilder from wpilib import Timer @@ -15,6 +23,7 @@ from .command import Command from .subsystem import Subsystem + class RamseteCommand(Command): """ A command that uses a RAMSETE controller (RamseteController) to follow a trajectory @@ -62,7 +71,7 @@ def __init__( 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 + 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 @@ -90,11 +99,11 @@ def __init__( # 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} ' + 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 @@ -109,7 +118,11 @@ 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 ) + ChassisSpeeds( + initial_state.velocity, + 0, + initial_state.curvature * initial_state.velocity, + ) ) self.prevSpeeds = initial_speeds self.timer.restart() @@ -135,29 +148,20 @@ def execute(self): if self.usePID: left_feedforward = self.feedforward.calculate( - left_speed_setpoint, - (left_speed_setpoint - self.prevSpeeds.left) / dt + 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 + (right_speed_setpoint - self.prevSpeeds.right) / dt, ) - left_output = ( - left_feedforward - + self.leftController.calculate( - self.wheelspeeds().left, - left_speed_setpoint - ) + 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 - ) + right_output = right_feedforward + self.rightController.calculate( + self.wheelspeeds().right, right_speed_setpoint ) else: left_output = left_speed_setpoint diff --git a/tests/test_ramsetecommand.py b/tests/test_ramsetecommand.py index ee6b69af..70e76511 100644 --- a/tests/test_ramsetecommand.py +++ b/tests/test_ramsetecommand.py @@ -48,7 +48,9 @@ def __init__(self): self.kMaxMetersPerSecond = 3.0 self.kMaxAccelerationMetersPerSecondSquared = 1.0 self.kEncoderCPR = 1024 - self.kEncoderDistancePerPulse = (self.kWheelDiameterMeters * math.pi) / self.kEncoderCPR + self.kEncoderDistancePerPulse = ( + self.kWheelDiameterMeters * math.pi + ) / self.kEncoderCPR self.command_kinematics: kinematics.DifferentialDriveKinematics = ( kinematics.DifferentialDriveKinematics(self.kTrackWidth) @@ -56,19 +58,21 @@ def __init__(self): self.command_voltage_constraint: constraints.DifferentialDriveVoltageConstraint = constraints.DifferentialDriveVoltageConstraint( controller.SimpleMotorFeedforwardMeters( - self.ksVolts, + self.ksVolts, self.kvVoltSecondsPerMeter, - self.kaVoltSecondsSquaredPerMeter - ), - self.command_kinematics, - 10 + 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)) + 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: @@ -84,7 +88,7 @@ def getCurrentWheelDistances(self) -> kinematics.DifferentialDriveWheelPositions def getRobotPose(self) -> geometry.Pose2d: positions = self.getCurrentWheelDistances() - self.command_odometry.update(self.angle, positions.left,positions.right) + self.command_odometry.update(self.angle, positions.left, positions.right) return self.command_odometry.getPose() @@ -111,12 +115,14 @@ def test_ramseteCommand( end_state = new_trajectory.sample(new_trajectory.totalTime()) command = commands2.RamseteCommand( - new_trajectory, + new_trajectory, fixture_data.getRobotPose, - controller.RamseteController(fixture_data.kRamseteB, fixture_data.kRamseteZeta), + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), fixture_data.command_kinematics, fixture_data.setWheelSpeeds, - subsystem + subsystem, ) fixture_data.timer.restart() @@ -139,4 +145,4 @@ def test_ramseteCommand( ) assert end_state.pose.Y() == pytest.approx( fixture_data.getRobotPose().Y(), fixture_data.kyTolerance - ) \ No newline at end of file + ) From 0072a3af71b92a85d348c6e4cc82dc9f3d1f5149 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 7 Dec 2023 13:09:15 -0500 Subject: [PATCH 03/10] Made PR requested changes as well as mypy detected errors --- commands2/ramsetecommand.py | 89 +++++++++++++++++-------------------- 1 file changed, 42 insertions(+), 47 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index 7d378ef7..3b6e5f26 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 +from typing import Callable, Union, Optional from wpimath.controller import ( PIDController, RamseteController, @@ -48,10 +48,10 @@ def __init__( 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, + feedforward: Optional[SimpleMotorFeedforwardMeters], + leftController: Optional[PIDController], + rightController: Optional[PIDController], + wheelSpeeds: Optional[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 @@ -82,20 +82,16 @@ def __init__( """ super().__init__() - self.timer = Timer() + self._timer = Timer() # Store all the requirements that are required - self.trajectory: Trajectory = trajectory + self.trajectory = trajectory self.pose = pose - self.follower: RamseteController = controller + self.follower = controller self.kinematics = kinematics - self.output: Callable[[float, float], None] = outputVolts - self.leftController = None - self.rightController = None - self.feedforward = None - self.wheelspeeds = None + self.output = outputVolts self.usePID = False - # Optional requirements checks. If any one of the optionl parameters are not None, then all the optional + # 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: @@ -109,78 +105,77 @@ def __init__( self.wheelspeeds: Callable[[], DifferentialDriveWheelSpeeds] = wheelSpeeds self.feedforward: SimpleMotorFeedforwardMeters = feedforward self.usePID = True - self.prevSpeeds = DifferentialDriveWheelSpeeds() - self.prevTime = -1.0 + self._prevSpeeds = DifferentialDriveWheelSpeeds() + self._prevTime = -1.0 self.addRequirements(*requirements) def initialize(self): - self.prevTime = -1 + self._prevTime = -1 initial_state = self.trajectory.sample(0) - initial_speeds = self.kinematics.toWheelSpeeds( + self._prevSpeeds = self.kinematics.toWheelSpeeds( ChassisSpeeds( initial_state.velocity, 0, initial_state.curvature * initial_state.velocity, ) ) - self.prevSpeeds = initial_speeds - self.timer.restart() + 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 + curTime = self._timer.get() + dt = curTime - self._prevTime - if self.prevTime < 0: + if self._prevTime < 0: self.output(0.0, 0.0) - self.prevTime = cur_time + self._prevTime = curTime return - target_wheel_speeds = self.kinematics.toWheelSpeeds( - self.follower.calculate(self.pose(), self.trajectory.sample(cur_time)) + targetWheelSpeeds = self.kinematics.toWheelSpeeds( + self.follower.calculate(self.pose(), self.trajectory.sample(curTime)) ) - left_speed_setpoint = target_wheel_speeds.left - right_speed_setpoint = target_wheel_speeds.right + leftSpeedSetpoint = targetWheelSpeeds.left + rightSpeedSetpoint = targetWheelSpeeds.right if self.usePID: - left_feedforward = self.feedforward.calculate( - left_speed_setpoint, (left_speed_setpoint - self.prevSpeeds.left) / dt + leftFeedforward = self.feedforward.calculate( + leftSpeedSetpoint, (leftSpeedSetpoint - self._prevSpeeds.left) / dt ) - right_feedforward = self.feedforward.calculate( - right_speed_setpoint, - (right_speed_setpoint - self.prevSpeeds.right) / dt, + rightFeedforward = self.feedforward.calculate( + rightSpeedSetpoint, + (rightSpeedSetpoint - self._prevSpeeds.right) / dt, ) - left_output = left_feedforward + self.leftController.calculate( - self.wheelspeeds().left, left_speed_setpoint + leftOutput = leftFeedforward + self.leftController.calculate( + self.wheelspeeds().left, leftSpeedSetpoint ) - right_output = right_feedforward + self.rightController.calculate( - self.wheelspeeds().right, right_speed_setpoint + rightOutput = rightFeedforward + self.rightController.calculate( + self.wheelspeeds().right, rightSpeedSetpoint ) else: - left_output = left_speed_setpoint - right_output = right_speed_setpoint + leftOutput = leftSpeedSetpoint + rightOutput = rightSpeedSetpoint - self.output(left_output, right_output) - self.prevSpeeds = target_wheel_speeds - self.prevTime = cur_time + self.output(leftOutput, rightOutput) + self._prevSpeeds = targetWheelSpeeds + self._prevTime = curTime - def end(self, interrupted): - self.timer.stop() + 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()) + 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) + builder.addDoubleProperty("leftVelocity", lambda: self._prevSpeeds.left, lambda *float: None) + builder.addDoubleProperty("rightVelocity", lambda: self._prevSpeeds.right, lambda *float: None) From 5cdd9a7885abbb14a93c9850b28487418841d7a8 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 7 Dec 2023 13:12:38 -0500 Subject: [PATCH 04/10] Re-formatted with Black --- commands2/ramsetecommand.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index 3b6e5f26..3e24ab19 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -89,7 +89,7 @@ def __init__( self.pose = pose self.follower = controller self.kinematics = kinematics - self.output = outputVolts + 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. @@ -177,5 +177,9 @@ def isFinished(self): 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) + builder.addDoubleProperty( + "leftVelocity", lambda: self._prevSpeeds.left, lambda *float: None + ) + builder.addDoubleProperty( + "rightVelocity", lambda: self._prevSpeeds.right, lambda *float: None + ) From fed8a3d3cd03ddb5c9b823a5b513dfe192ecf497 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 7 Dec 2023 13:19:18 -0500 Subject: [PATCH 05/10] Fixed failing tests. --- commands2/ramsetecommand.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index 3e24ab19..b6a79b8a 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -48,10 +48,10 @@ def __init__( kinematics: DifferentialDriveKinematics, outputVolts: Callable[[float, float], None], *requirements: Subsystem, - feedforward: Optional[SimpleMotorFeedforwardMeters], - leftController: Optional[PIDController], - rightController: Optional[PIDController], - wheelSpeeds: Optional[Callable[[], DifferentialDriveWheelSpeeds]], + 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 From ec8a0a0cf961e0770417bd093f97d3638e0991cb Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 7 Dec 2023 13:22:38 -0500 Subject: [PATCH 06/10] Fixed typing in assignment for Mypy failures. --- commands2/ramsetecommand.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index b6a79b8a..be9c839c 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -100,10 +100,10 @@ def __init__( 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.leftController = leftController + self.rightController = rightController + self.wheelspeeds = wheelSpeeds + self.feedforward = feedforward self.usePID = True self._prevSpeeds = DifferentialDriveWheelSpeeds() self._prevTime = -1.0 From 533027ec790c3b28989d75146d8fda4d29e8e11c Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 28 Dec 2023 13:30:36 -0500 Subject: [PATCH 07/10] Updated implementation to use the overload typing annotation. Adds more tests to check for correct argument parsing in constructor --- commands2/ramsetecommand.py | 139 ++++++++++++--- tests/test_ramsetecommand.py | 316 ++++++++++++++++++++++++++++++++++- 2 files changed, 428 insertions(+), 27 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index be9c839c..3c3a4b10 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,50 @@ 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: 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, - outputVolts: Callable[[float, float], None], - *requirements: Subsystem, + 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, @@ -65,10 +98,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,26 +151,52 @@ def __init__( self.pose = pose self.follower = controller self.kinematics = kinematics - self.output = outputVolts - self.usePID = False + 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 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} " ) - - self.leftController = leftController - self.rightController = rightController - self.wheelspeeds = wheelSpeeds - self.feedforward = feedforward self.usePID = True - self._prevSpeeds = DifferentialDriveWheelSpeeds() - self._prevTime = -1.0 + else: + self.usePID = False - self.addRequirements(*requirements) + # 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 @@ -130,7 +218,10 @@ def execute(self): dt = curTime - self._prevTime if self._prevTime < 0: - self.output(0.0, 0.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 @@ -158,11 +249,12 @@ def execute(self): rightOutput = rightFeedforward + self.rightController.calculate( self.wheelspeeds().right, rightSpeedSetpoint ) + self.outputVolts(leftOutput, rightOutput) else: leftOutput = leftSpeedSetpoint rightOutput = rightSpeedSetpoint + self.outputMPS(leftOutput, rightOutput) - self.output(leftOutput, rightOutput) self._prevSpeeds = targetWheelSpeeds self._prevTime = curTime @@ -170,7 +262,10 @@ def end(self, interrupted: bool): self._timer.stop() if interrupted: - self.output(0.0, 0.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) def isFinished(self): return self._timer.hasElapsed(self.trajectory.totalTime()) 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() From 0f9447a2e8be7d10c272e38a521651241c460fe5 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 28 Dec 2023 16:14:27 -0500 Subject: [PATCH 08/10] Implemented RamseteCommand with overtake and beartype for better multiple dispatch. robotpy/robotpy-commands-v2#28. --- commands2/ramsetecommand.py | 162 ++++++++++++----------------------- setup.py | 7 +- tests/test_ramsetecommand.py | 76 +++++----------- 3 files changed, 84 insertions(+), 161 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index 3c3a4b10..c6a75e0b 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -4,6 +4,7 @@ from __future__ import annotations from typing import Callable, Union, Optional, Tuple, overload +from overtake import overtake from wpimath.controller import ( PIDController, RamseteController, @@ -47,44 +48,7 @@ def __init__( 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, + 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 @@ -99,17 +63,37 @@ def __init__( :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__() - 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` - + 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 - Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID + @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. @@ -122,20 +106,7 @@ def __init__( :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 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. @@ -151,53 +122,36 @@ def __init__( self.pose = pose self.follower = controller self.kinematics = kinematics - self.outputMPS = outputMPS - self.outputVolts = outputVolts + self.output = output 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 - + 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) @@ -218,10 +172,7 @@ def execute(self): 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.output(0.0, 0.0) self._prevTime = curTime return @@ -249,11 +200,11 @@ def execute(self): rightOutput = rightFeedforward + self.rightController.calculate( self.wheelspeeds().right, rightSpeedSetpoint ) - self.outputVolts(leftOutput, rightOutput) + self.output(leftOutput, rightOutput) else: leftOutput = leftSpeedSetpoint rightOutput = rightSpeedSetpoint - self.outputMPS(leftOutput, rightOutput) + self.output(leftOutput, rightOutput) self._prevSpeeds = targetWheelSpeeds self._prevTime = curTime @@ -262,10 +213,7 @@ 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) + self.output(0.0, 0.0) def isFinished(self): return self._timer.hasElapsed(self.trajectory.totalTime()) diff --git a/setup.py b/setup.py index b1c069cb..9111ea9c 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 index e383781b..604380d5 100644 --- a/tests/test_ramsetecommand.py +++ b/tests/test_ramsetecommand.py @@ -83,7 +83,7 @@ def setWheelSpeedsMPS( self.rightSpeed = rightspeed def setWheelSpeedsVolts( - self, leftVolts: units.volt_seconds, rightVolts: units.volt_seconds + self, leftVolts: units.volts, rightVolts: units.volts ) -> None: self.leftSpeed = leftVolts self.rightSpeed = rightVolts @@ -126,7 +126,7 @@ def test_rameseteRaisesNoOutputRaises( ) end_state = new_trajectory.sample(new_trajectory.totalTime()) - with pytest.raises(RuntimeError): + with pytest.raises(Exception): command = commands2.RamseteCommand( new_trajectory, fixture_data.getRobotPose, @@ -138,37 +138,6 @@ def test_rameseteRaisesNoOutputRaises( ) -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 ): @@ -193,7 +162,7 @@ def test_rameseteRaisesOnlyFeedForward( ) ) - with pytest.raises(RuntimeError): + with pytest.raises(Exception): command = commands2.RamseteCommand( new_trajectory, fixture_data.getRobotPose, @@ -202,8 +171,8 @@ def test_rameseteRaisesOnlyFeedForward( ), fixture_data.command_kinematics, subsystem, - outputMPS=fixture_data.setWheelSpeedsMPS, - feedforward=feedforward_var, + fixture_data.setWheelSpeedsMPS, + feedforward_var, ) @@ -232,7 +201,7 @@ def test_rameseteRaisesFeedForwardAndLeft( ) left_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) - with pytest.raises(RuntimeError): + with pytest.raises(Exception): command = commands2.RamseteCommand( new_trajectory, fixture_data.getRobotPose, @@ -241,9 +210,9 @@ def test_rameseteRaisesFeedForwardAndLeft( ), fixture_data.command_kinematics, subsystem, - outputMPS=fixture_data.setWheelSpeedsMPS, - feedforward=feedforward_var, - leftController=left_pid, + fixture_data.setWheelSpeedsMPS, + feedforward_var, + left_pid, ) @@ -271,9 +240,9 @@ def test_rameseteRaisesFeedForwardRightAndLeft( ) ) left_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) - rightt_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) + right_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) - with pytest.raises(RuntimeError): + with pytest.raises(Exception): command = commands2.RamseteCommand( new_trajectory, fixture_data.getRobotPose, @@ -282,10 +251,10 @@ def test_rameseteRaisesFeedForwardRightAndLeft( ), fixture_data.command_kinematics, subsystem, - outputMPS=fixture_data.setWheelSpeedsMPS, - feedforward=feedforward_var, - leftController=left_pid, - rightController=rightt_pid, + fixture_data.setWheelSpeedsMPS, + feedforward_var, + left_pid, + right_pid, ) @@ -314,7 +283,7 @@ def test_ramseteCommandMPSReachesDestination( ), fixture_data.command_kinematics, subsystem, - outputMPS=fixture_data.setWheelSpeedsMPS, + fixture_data.setWheelSpeedsMPS, ) fixture_data.timer.restart() @@ -365,7 +334,7 @@ def test_ramseteCommandVoltsReachesDestination( ), fixture_data.command_kinematics, subsystem, - outputMPS=fixture_data.setWheelSpeedsVolts, + fixture_data.setWheelSpeedsVolts, ) fixture_data.timer.restart() @@ -416,6 +385,7 @@ def test_ramseteCommandPIDReachesDestination( ) 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, @@ -424,11 +394,11 @@ def test_ramseteCommandPIDReachesDestination( ), fixture_data.command_kinematics, subsystem, - outputVolts=fixture_data.setWheelSpeedsVolts, - feedforward=feedforward_var, - leftController=left_pid, - rightController=rightt_pid, - wheelSpeeds=fixture_data.getWheelSpeeds, + fixture_data.setWheelSpeedsVolts, + feedforward_var, + left_pid, + rightt_pid, + fixture_data.getWheelSpeeds, ) fixture_data.timer.restart() From 5517b58ef4d31519a5338929761ca495270a5ebf Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 29 Dec 2023 09:50:44 -0500 Subject: [PATCH 09/10] Took immport of overload from typing_extensions instead of typing. --- commands2/ramsetecommand.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index c6a75e0b..fd33dee6 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -3,7 +3,8 @@ # 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 typing import Callable, Union, Optional, Tuple +from typing_extensions import overload from overtake import overtake from wpimath.controller import ( PIDController, From 9739b5fc466832075094a8e6a31611f117e3ac5f Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Sat, 30 Dec 2023 14:48:00 -0500 Subject: [PATCH 10/10] Pinned version of experimental libraries and renamed class variables --- commands2/ramsetecommand.py | 70 ++++++++++++++++++------------------- setup.py | 4 +-- 2 files changed, 37 insertions(+), 37 deletions(-) diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py index fd33dee6..922b6a7f 100644 --- a/commands2/ramsetecommand.py +++ b/commands2/ramsetecommand.py @@ -69,12 +69,12 @@ def __init__( super().__init__() self._timer = Timer() - self.trajectory = trajectory - self.pose = pose - self.follower = controller - self.kinematics = kinematics - self.output = output - self.usePID = False + 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. @@ -119,18 +119,18 @@ def __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._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 + 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. @@ -155,8 +155,8 @@ def __init__( def initialize(self): self._prevTime = -1 - initial_state = self.trajectory.sample(0) - self._prevSpeeds = self.kinematics.toWheelSpeeds( + initial_state = self._trajectory.sample(0) + self._prevSpeeds = self._kinematics.toWheelSpeeds( ChassisSpeeds( initial_state.velocity, 0, @@ -164,48 +164,48 @@ def initialize(self): ) ) self._timer.restart() - if self.usePID: - self.leftController.reset() - self.rightController.reset() + 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._output(0.0, 0.0) self._prevTime = curTime return - targetWheelSpeeds = self.kinematics.toWheelSpeeds( - self.follower.calculate(self.pose(), self.trajectory.sample(curTime)) + 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( + if self._usePID: + leftFeedforward = self._feedforward.calculate( leftSpeedSetpoint, (leftSpeedSetpoint - self._prevSpeeds.left) / dt ) - rightFeedforward = self.feedforward.calculate( + rightFeedforward = self._feedforward.calculate( rightSpeedSetpoint, (rightSpeedSetpoint - self._prevSpeeds.right) / dt, ) - leftOutput = leftFeedforward + self.leftController.calculate( - self.wheelspeeds().left, leftSpeedSetpoint + leftOutput = leftFeedforward + self._leftController.calculate( + self._wheelspeeds().left, leftSpeedSetpoint ) - rightOutput = rightFeedforward + self.rightController.calculate( - self.wheelspeeds().right, rightSpeedSetpoint + rightOutput = rightFeedforward + self._rightController.calculate( + self._wheelspeeds().right, rightSpeedSetpoint ) - self.output(leftOutput, rightOutput) + self._output(leftOutput, rightOutput) else: leftOutput = leftSpeedSetpoint rightOutput = rightSpeedSetpoint - self.output(leftOutput, rightOutput) + self._output(leftOutput, rightOutput) self._prevSpeeds = targetWheelSpeeds self._prevTime = curTime @@ -214,10 +214,10 @@ def end(self, interrupted: bool): self._timer.stop() if interrupted: - self.output(0.0, 0.0) + self._output(0.0, 0.0) def isFinished(self): - return self._timer.hasElapsed(self.trajectory.totalTime()) + return self._timer.hasElapsed(self._trajectory.totalTime()) def initSendable(self, builder: SendableBuilder): super().initSendable(builder) diff --git a/setup.py b/setup.py index 9111ea9c..99b6e032 100644 --- a/setup.py +++ b/setup.py @@ -14,8 +14,8 @@ install_requires=[ "wpilib<2025,>=2024.0.0b2", "typing_extensions>=4.1.0,<5", - "overtake>=0.4.0", - "beartype>=0.16.4", + "overtake~=0.4.0", + "beartype~=0.16.4", ], license="BSD-3-Clause", python_requires=">=3.8",