From 8bc6e39f18424586b4cd4f6042ebfe6c232a47b8 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 8 Dec 2023 11:26:11 -0500 Subject: [PATCH 01/14] Adds Swerve2/3/4/6ControllerCommand to Commands2 --- commands2/__init__.py | 2 + commands2/swervecontrollercommand.py | 143 +++++ tests/test_swervecontrollercommand.py | 880 ++++++++++++++++++++++++++ 3 files changed, 1025 insertions(+) create mode 100644 commands2/swervecontrollercommand.py create mode 100644 tests/test_swervecontrollercommand.py diff --git a/commands2/__init__.py b/commands2/__init__.py index cc8e6a4c..936d0877 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -41,6 +41,7 @@ from .sequentialcommandgroup import SequentialCommandGroup from .startendcommand import StartEndCommand from .subsystem import Subsystem +from .swervecontrollercommand import SwerveControllerCommand from .timedcommandrobot import TimedCommandRobot from .waitcommand import WaitCommand from .waituntilcommand import WaitUntilCommand @@ -72,6 +73,7 @@ "SequentialCommandGroup", "StartEndCommand", "Subsystem", + "SwerveControllerCommand", "TimedCommandRobot", "WaitCommand", "WaitUntilCommand", diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py new file mode 100644 index 00000000..d6d69142 --- /dev/null +++ b/commands2/swervecontrollercommand.py @@ -0,0 +1,143 @@ +# 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, Optional, Union + +from wpimath.controller import ( + HolonomicDriveController, + PIDController, + ProfiledPIDController, +) +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.kinematics import ( + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + SwerveModuleState, +) +from wpimath.trajectory import Trajectory +from wpilib import Timer + +from .command import Command +from .subsystem import Subsystem + + +class SwerveControllerCommand(Command): + """ + A command that uses two PID controllers (PIDController) and a ProfiledPIDController + (ProfiledPIDController) to follow a trajectory (Trajectory) with a swerve drive. + + This command outputs the raw desired Swerve Module States (SwerveModuleState) in an + array. The desired wheel and module rotation velocities should be taken from those and used in + velocity PIDs. + + The robot angle controller does not follow the angle given by the trajectory but rather goes + to the angle given in the final state of the trajectory. + + This class is provided by the NewCommands VendorDep + """ + + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + outputModuleStates: Callable[[SwerveModuleState], None], + *requirements: Subsystem, + controller: Optional[HolonomicDriveController] = None, + xController: Optional[PIDController] = None, + yController: Optional[PIDController] = None, + thetaController: Optional[ProfiledPIDController] = None, + desiredRotation: Callable[[], Rotation2d] = None, + ): + """Constructs a new SwerveControllerCommand that when executed will follow the provided + trajectory. This command will not return output voltages but rather raw module states from the + position controllers which need to be put into a velocity PID. + + Note: The controllers 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. + + Note 2: The final rotation of the robot will be set to the rotation of the final pose in the + trajectory. The robot will not follow the rotations from the poses at each timestep. If + alternate rotation behavior is desired, the other constructor with a supplier for rotation + should be used. + + Note 3: The constructor requires 5 arguments: Trajectory, Pose, Kinematics, OutputConsumer, and + the Subsystem. For functionality, a sixth component is required: `HolonomicDriveController`. To satisfy + this sixth requirement, the caller can pass in a `HolonomicDriveController` object, or pass in the + combination of X, Y, Theta PID controllers with a Supplier of `Rotation2d` objects representing the + desired rotation at the end of the trajectory. If the caller doesn't supply a constructed controller + or PID controllers such that a `HolonomicDriveController` can be constructed a `RuntimeError` will + be raised. + + :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 kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + + Optional Requirements + :param controller: HolonomicDriveController for the drivetrain + :param xController: Trajectory Tracker PID controller for the robot's x position + :param yController: Trajectory Tracker PID controller for the robot's y position + :param thetaController: Trajectory Tracker PID controller for the angle for the robot + :param desiredRotation: The angle that the drivetrain should be facing. This is sampled at + each time step + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + + # Parse the controller parameters combination. If the controller is passed in constructed, + # ignore the PID controllers. If it's None, parse them. + if controller is not None: + self._controller = controller + else: + # Verify the PID controller combination + if (xController and yController and thetaController) is None: + raise RuntimeError( + f"Failed to instantiate the Swerve2ControllerCommand: Could not create HolonomicDriveController from PID requirements" + ) + + self._controller = HolonomicDriveController( + xController, yController, thetaController + ) + + # If the desired rotation isn't provided, just take the final rotation from the trajectory + if desiredRotation is not None: + self._desiredRotation = desiredRotation + else: + self._desiredRotation = self._trajectory.states()[-1].pose.rotation + self.addRequirements(*requirements) + self._timer = Timer() + + def initialize(self): + self._timer.restart() + + def execute(self): + curTime = self._timer.get() + desiredState = self._trajectory.sample(curTime) + + targetChassisSpeeds = self._controller.calculate( + self._pose(), desiredState, self._desiredRotation() + ) + targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds) + + self._outputModuleStates(targetModuleStates) + + def end(self, interrupted): + self._timer.stop() + + def isFinished(self): + return self._timer.hasElapsed(self._trajectory.totalTime()) diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py new file mode 100644 index 00000000..3948076e --- /dev/null +++ b/tests/test_swervecontrollercommand.py @@ -0,0 +1,880 @@ +# 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, Tuple +import math + +import wpimath.controller as controller +import wpimath.trajectory as trajectory +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 + +TWO = int(2) +THREE = int(3) +FOUR = int(4) +SIX = int(6) +MISMATCHED_KINEMATICS_AND_ODOMETRY = int(101) +INCOMPLETE_PID_CLASSES = int(102) + + +class SwerveControllerCommandTestDataFixtures: + def __init__(self, selector: int): + self._timer = Timer() + self._angle: geometry.Rotation2d = geometry.Rotation2d(0) + + self._kxTolerance = 1 / 12.0 + self._kyTolerance = 1 / 12.0 + self._kAngularTolerance = 1 / 12.0 + self._kWheelBase = 0.5 + self._kTrackWidth = 0.5 + + # The module positions and states start empty and will be populated below in the selector + # self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] + self._modulePositions: List[kinematics.SwerveModulePosition] = [] + self._moduleStates: List[kinematics.SwerveModuleState] = [] + + # Setup PID controllers, but if an error test case is requested, make sure it provides + # data that should break the command instantiation + if selector != INCOMPLETE_PID_CLASSES: + self._xController = controller.PIDController(0.6, 0, 0) + self._yController = controller.PIDController(0.6, 0, 0) + constraints = trajectory.TrapezoidProfileRadians.Constraints( + 3 * math.pi, math.pi + ) + self._rotationController = controller.ProfiledPIDControllerRadians( + 1, 0, 0, constraints + ) + + self._holonomic = controller.HolonomicDriveController( + self._xController, self._yController, self._rotationController + ) + else: + self._xController = None + self._holonomic = None + + if selector == TWO: + self._kinematics = kinematics.SwerveDrive2Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive2Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == THREE: + self._kinematics = kinematics.SwerveDrive3Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive3Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == FOUR: + self._kinematics = kinematics.SwerveDrive4Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive4Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == SIX: + self._kinematics = kinematics.SwerveDrive6Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive6Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == MISMATCHED_KINEMATICS_AND_ODOMETRY: + self._kinematics = kinematics.SwerveDrive2Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive6Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + else: + raise RuntimeError(f"Invalid SwerveKinematics selector: {selector}") + + def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: + self._moduleStates = states + + def getRobotPose(self) -> geometry.Pose2d: + if isinstance(self._odometry, kinematics.SwerveDrive2Odometry): + self._odometry.update( + self._angle, self._modulePositions[0], self._modulePositions[1] + ) + elif isinstance(self._odometry, kinematics.SwerveDrive3Odometry): + self._odometry.update( + self._angle, + self._modulePositions[0], + self._modulePositions[1], + self._modulePositions[2], + ) + elif isinstance(self._odometry, kinematics.SwerveDrive4Odometry): + self._odometry.update( + self._angle, + self._modulePositions[0], + self._modulePositions[1], + self._modulePositions[2], + self._modulePositions[3], + ) + elif isinstance(self._odometry, kinematics.SwerveDrive6Odometry): + self._odometry.update( + self._angle, + self._modulePositions[0], + self._modulePositions[1], + self._modulePositions[2], + self._modulePositions[3], + self._modulePositions[4], + self._modulePositions[5], + ) + + return self._odometry.getPose() + + def getRotationHeadingZero(self) -> geometry.Rotation2d: + return geometry.Rotation2d() + + +@pytest.fixture() +def get_swerve_controller_data() -> SwerveControllerCommandTestDataFixtures: + def _method(selector): + return SwerveControllerCommandTestDataFixtures(selector) + + return _method + + +def test_SwerveControllerCommand2Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand2PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand3Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand3PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand4Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand4PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerDesiredRotationNonNull( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerMismatchedKinematicsAndOdometry( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(TypeError): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(MISMATCHED_KINEMATICS_AND_ODOMETRY) + ) + + +def test_SwerveControllerIncompletePID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(RuntimeError): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(INCOMPLETE_PID_CLASSES) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, + ) From d883093d64da5b927de8b3453e71e5117f596155 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 8 Dec 2023 12:14:26 -0500 Subject: [PATCH 02/14] Fixes for mypy errors --- commands2/swervecontrollercommand.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index d6d69142..fabeeb05 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -7,7 +7,7 @@ from wpimath.controller import ( HolonomicDriveController, PIDController, - ProfiledPIDController, + ProfiledPIDControllerRadians, ) from wpimath.geometry import Pose2d, Rotation2d from wpimath.kinematics import ( @@ -54,8 +54,8 @@ def __init__( controller: Optional[HolonomicDriveController] = None, xController: Optional[PIDController] = None, yController: Optional[PIDController] = None, - thetaController: Optional[ProfiledPIDController] = None, - desiredRotation: Callable[[], Rotation2d] = None, + thetaController: Optional[ProfiledPIDControllerRadians] = None, + desiredRotation: Optional[Callable[[], Rotation2d]] = None, ): """Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but rather raw module states from the @@ -110,9 +110,12 @@ def __init__( f"Failed to instantiate the Swerve2ControllerCommand: Could not create HolonomicDriveController from PID requirements" ) - self._controller = HolonomicDriveController( - xController, yController, thetaController - ) + # re-type variables to fix MyPy error: + # Argument 1 to "HolonomicDriveController" has incompatible type "PIDController | None"; expected "PIDController" [arg-type] + x: PIDController = xController + y: PIDController = yController + theta: ProfiledPIDControllerRadians = thetaController + self._controller = HolonomicDriveController( x, y, theta ) # If the desired rotation isn't provided, just take the final rotation from the trajectory if desiredRotation is not None: From 645163a51e54b64a68b008fdcb044cbabc2e9be9 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 8 Dec 2023 12:26:31 -0500 Subject: [PATCH 03/14] Fixes broken test run for incomplete pid --- commands2/swervecontrollercommand.py | 7 +------ tests/test_swervecontrollercommand.py | 12 +++++++++--- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index fabeeb05..afd0f552 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -110,12 +110,7 @@ def __init__( f"Failed to instantiate the Swerve2ControllerCommand: Could not create HolonomicDriveController from PID requirements" ) - # re-type variables to fix MyPy error: - # Argument 1 to "HolonomicDriveController" has incompatible type "PIDController | None"; expected "PIDController" [arg-type] - x: PIDController = xController - y: PIDController = yController - theta: ProfiledPIDControllerRadians = thetaController - self._controller = HolonomicDriveController( x, y, theta ) + self._controller = HolonomicDriveController( xController, yController, thetaController ) # If the desired rotation isn't provided, just take the final rotation from the trajectory if desiredRotation is not None: diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index 3948076e..9df2a97a 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -61,9 +61,16 @@ def __init__(self, selector: int): ) else: self._xController = None + self._yController = controller.PIDController(0.6, 0, 0) + constraints = trajectory.TrapezoidProfileRadians.Constraints( + 3 * math.pi, math.pi + ) + self._rotationController = controller.ProfiledPIDControllerRadians( + 1, 0, 0, constraints + ) self._holonomic = None - if selector == TWO: + if (selector == TWO) or (selector == INCOMPLETE_PID_CLASSES): self._kinematics = kinematics.SwerveDrive2Kinematics( geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), @@ -208,8 +215,7 @@ def __init__(self, selector: int): self._modulePositions, geometry.Pose2d(0, 0, geometry.Rotation2d(0)), ) - else: - raise RuntimeError(f"Invalid SwerveKinematics selector: {selector}") + def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: self._moduleStates = states From 1042681cf77754666a779f8f3d559e6c8d6392d4 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 8 Dec 2023 12:31:12 -0500 Subject: [PATCH 04/14] Experimenting with mypy assignment error --- commands2/swervecontrollercommand.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index afd0f552..4c45b1f9 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -52,7 +52,7 @@ def __init__( outputModuleStates: Callable[[SwerveModuleState], None], *requirements: Subsystem, controller: Optional[HolonomicDriveController] = None, - xController: Optional[PIDController] = None, + xController: Union[PIDController, None] = None, yController: Optional[PIDController] = None, thetaController: Optional[ProfiledPIDControllerRadians] = None, desiredRotation: Optional[Callable[[], Rotation2d]] = None, From 8464e6f5fb79c5b4b7ed7dfe81bb005ae274cb73 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 8 Dec 2023 12:34:36 -0500 Subject: [PATCH 05/14] Ignoring mypy type error with explicit annotation --- commands2/swervecontrollercommand.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index 4c45b1f9..dc83ac1d 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -52,7 +52,7 @@ def __init__( outputModuleStates: Callable[[SwerveModuleState], None], *requirements: Subsystem, controller: Optional[HolonomicDriveController] = None, - xController: Union[PIDController, None] = None, + xController: Optional[PIDController] = None, yController: Optional[PIDController] = None, thetaController: Optional[ProfiledPIDControllerRadians] = None, desiredRotation: Optional[Callable[[], Rotation2d]] = None, @@ -110,7 +110,11 @@ def __init__( f"Failed to instantiate the Swerve2ControllerCommand: Could not create HolonomicDriveController from PID requirements" ) - self._controller = HolonomicDriveController( xController, yController, thetaController ) + # Adding the mypy type error annotation since it incorrectly is giving an error that the type is: + # PIDController | None expected PIDController + # The statement is true, but ignore the error because the None type check on above makes sure the + # typing is correct. + self._controller = HolonomicDriveController( xController, yController, thetaController ) # type: ignore # If the desired rotation isn't provided, just take the final rotation from the trajectory if desiredRotation is not None: From 3c746624b15ca115a823d19ee0eb857d083b273d Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 8 Dec 2023 12:35:20 -0500 Subject: [PATCH 06/14] Re-formatted with black --- commands2/swervecontrollercommand.py | 4 ++-- tests/test_swervecontrollercommand.py | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index dc83ac1d..f3e294f5 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -112,9 +112,9 @@ def __init__( # Adding the mypy type error annotation since it incorrectly is giving an error that the type is: # PIDController | None expected PIDController - # The statement is true, but ignore the error because the None type check on above makes sure the + # The statement is true, but ignore the error because the None type check on above makes sure the # typing is correct. - self._controller = HolonomicDriveController( xController, yController, thetaController ) # type: ignore + self._controller = HolonomicDriveController(xController, yController, thetaController) # type: ignore # If the desired rotation isn't provided, just take the final rotation from the trajectory if desiredRotation is not None: diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index 9df2a97a..839e9726 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -216,7 +216,6 @@ def __init__(self, selector: int): geometry.Pose2d(0, 0, geometry.Rotation2d(0)), ) - def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: self._moduleStates = states From 92455c0531dc8db5f023db5b8948e73a65c32aef Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Mon, 11 Dec 2023 12:06:54 -0500 Subject: [PATCH 07/14] Added Typing.overload to constructor definitions, forced kwargs for optional contructor arguments. --- commands2/swervecontrollercommand.py | 215 +++++++++-- tests/test_swervecontrollercommand.py | 511 ++++++++++++++++++++++++-- 2 files changed, 659 insertions(+), 67 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index f3e294f5..a4b180ec 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -2,7 +2,7 @@ # 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, Optional, Union +from typing import Callable, Optional, Union, overload, Tuple from wpimath.controller import ( HolonomicDriveController, @@ -39,6 +39,7 @@ class SwerveControllerCommand(Command): This class is provided by the NewCommands VendorDep """ + @overload def __init__( self, trajectory: Trajectory, @@ -50,32 +51,133 @@ def __init__( SwerveDrive6Kinematics, ], outputModuleStates: Callable[[SwerveModuleState], None], - *requirements: Subsystem, - controller: Optional[HolonomicDriveController] = None, - xController: Optional[PIDController] = None, - yController: Optional[PIDController] = None, - thetaController: Optional[ProfiledPIDControllerRadians] = None, - desiredRotation: Optional[Callable[[], Rotation2d]] = None, - ): - """Constructs a new SwerveControllerCommand that when executed will follow the provided - trajectory. This command will not return output voltages but rather raw module states from the - position controllers which need to be put into a velocity PID. - - Note: The controllers 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. - - Note 2: The final rotation of the robot will be set to the rotation of the final pose in the - trajectory. The robot will not follow the rotations from the poses at each timestep. If - alternate rotation behavior is desired, the other constructor with a supplier for rotation - should be used. - - Note 3: The constructor requires 5 arguments: Trajectory, Pose, Kinematics, OutputConsumer, and - the Subsystem. For functionality, a sixth component is required: `HolonomicDriveController`. To satisfy - this sixth requirement, the caller can pass in a `HolonomicDriveController` object, or pass in the - combination of X, Y, Theta PID controllers with a Supplier of `Rotation2d` objects representing the - desired rotation at the end of the trajectory. If the caller doesn't supply a constructed controller - or PID controllers such that a `HolonomicDriveController` can be constructed a `RuntimeError` will - be raised. + requirements: Tuple[Subsystem], + *, + xController: PIDController, + yController: PIDController, + thetaController: ProfiledPIDControllerRadians, + desiredRotation: Callable[[], Rotation2d], + ) -> None: + ... + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + *, + xController: PIDController, + yController: PIDController, + thetaController: ProfiledPIDControllerRadians, + ) -> None: + ... + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + *, + controller: HolonomicDriveController, + desiredRotation: Callable[[], Rotation2d], + ) -> None: + ... + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + *, + controller: HolonomicDriveController, + ) -> None: + ... + """ + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers 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 kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + :param xController: The Trajectory Tracker PID controller + for the robot's x position. + :param yController: The Trajectory Tracker PID controller + for the robot's y position. + :param thetaController: The Trajectory Tracker PID controller + for angle for the robot. + :param desiredRotation: The angle that the drivetrain should be + facing. This is sampled at each time step. + + + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers 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. + + Note 2: The final rotation of the robot will be set to the rotation of + the final pose in the trajectory. The robot will not follow the rotations + from the poses at each timestep. If alternate rotation behavior is desired, + the other constructor with a supplier for rotation should be used. + + :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 kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + :param xController: The Trajectory Tracker PID controller + for the robot's x position. + :param yController: The Trajectory Tracker PID controller + for the robot's y position. + :param thetaController: The Trajectory Tracker PID controller + for angle for the robot. + + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers 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 @@ -84,15 +186,54 @@ def __init__( SwerveKinematics. :param outputModuleStates: The raw output module states from the position controllers. :param requirements: The subsystems to require. + :param controller: The HolonomicDriveController for the drivetrain. + :param desiredRotation: The angle that the drivetrain should be + facing. This is sampled at each time step. + + + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. - Optional Requirements - :param controller: HolonomicDriveController for the drivetrain - :param xController: Trajectory Tracker PID controller for the robot's x position - :param yController: Trajectory Tracker PID controller for the robot's y position - :param thetaController: Trajectory Tracker PID controller for the angle for the robot - :param desiredRotation: The angle that the drivetrain should be facing. This is sampled at - each time step + Note: The controllers 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. + + Note 2: The final rotation of the robot will be set to the rotation of + the final pose in the trajectory. The robot will not follow the rotations + from the poses at each timestep. If alternate rotation behavior is desired, + the other constructor with a supplier for rotation should be used. + + :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 kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + :param controller: The HolonomicDriveController for the drivetrain. """ + + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + *, + controller: Optional[HolonomicDriveController] = None, + xController: Optional[PIDController] = None, + yController: Optional[PIDController] = None, + thetaController: Optional[ProfiledPIDControllerRadians] = None, + desiredRotation: Optional[Callable[[], Rotation2d]] = None, + ): super().__init__() self._trajectory = trajectory self._pose = pose @@ -121,7 +262,11 @@ def __init__( self._desiredRotation = desiredRotation else: self._desiredRotation = self._trajectory.states()[-1].pose.rotation - self.addRequirements(*requirements) + self.addRequirements( + # Ignoring type due to mypy stating the addRequirements was expecting a Subsystem but got an iterable of a subsystem. + # The iterable of the subsystem was to be able to force optional kwargs for the users when overloading the constructor. + requirements, # type: ignore + ) self._timer = Timer() def initialize(self): diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index 839e9726..b1c728d5 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -264,6 +264,58 @@ def _method(selector): return _method +def test_SwerveControllerMismatchedKinematicsAndOdometry( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(TypeError): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(MISMATCHED_KINEMATICS_AND_ODOMETRY) + ) + + +def test_SwerveControllerIncompletePID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(RuntimeError): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(INCOMPLETE_PID_CLASSES) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + def test_SwerveControllerCommand2Holonomic( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): @@ -326,6 +378,69 @@ def test_SwerveControllerCommand2Holonomic( ) +def test_SwerveControllerCommand2HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + def test_SwerveControllerCommand2PID( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): @@ -390,6 +505,71 @@ def test_SwerveControllerCommand2PID( ) +def test_SwerveControllerCommand2PIDWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + def test_SwerveControllerCommand3Holonomic( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): @@ -452,6 +632,69 @@ def test_SwerveControllerCommand3Holonomic( ) +def test_SwerveControllerCommand3HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + def test_SwerveControllerCommand3PID( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): @@ -516,6 +759,71 @@ def test_SwerveControllerCommand3PID( ) +def test_SwerveControllerCommand3PIDWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + def test_SwerveControllerCommand4Holonomic( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): @@ -578,6 +886,69 @@ def test_SwerveControllerCommand4Holonomic( ) +def test_SwerveControllerCommand4HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + controller=fixture_data._holonomic, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + def test_SwerveControllerCommand4PID( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): @@ -642,7 +1013,7 @@ def test_SwerveControllerCommand4PID( ) -def test_SwerveControllerCommand6Holonomic( +def test_SwerveControllerCommand4PIDWithRotation( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): with ManualSimTime() as sim: @@ -657,7 +1028,7 @@ def test_SwerveControllerCommand6Holonomic( end_state = new_trajectory.sample(new_trajectory.totalTime()) fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(SIX) + get_swerve_controller_data(FOUR) ) command = commands2.SwerveControllerCommand( @@ -666,7 +1037,10 @@ def test_SwerveControllerCommand6Holonomic( fixture_data._kinematics, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -704,7 +1078,7 @@ def test_SwerveControllerCommand6Holonomic( ) -def test_SwerveControllerCommand6PID( +def test_SwerveControllerCommand6Holonomic( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): with ManualSimTime() as sim: @@ -728,9 +1102,7 @@ def test_SwerveControllerCommand6PID( fixture_data._kinematics, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, + controller=fixture_data._holonomic, ) fixture_data._timer.restart() @@ -768,7 +1140,7 @@ def test_SwerveControllerCommand6PID( ) -def test_SwerveControllerDesiredRotationNonNull( +def test_SwerveControllerCommand6HolonomicWithRotation( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): with ManualSimTime() as sim: @@ -792,9 +1164,7 @@ def test_SwerveControllerDesiredRotationNonNull( fixture_data._kinematics, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, + controller=fixture_data._holonomic, desiredRotation=fixture_data.getRotationHeadingZero, ) @@ -833,7 +1203,7 @@ def test_SwerveControllerDesiredRotationNonNull( ) -def test_SwerveControllerMismatchedKinematicsAndOdometry( +def test_SwerveControllerCommand6PID( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): with ManualSimTime() as sim: @@ -847,13 +1217,57 @@ def test_SwerveControllerMismatchedKinematicsAndOdometry( ) end_state = new_trajectory.sample(new_trajectory.totalTime()) - with pytest.raises(TypeError): - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(MISMATCHED_KINEMATICS_AND_ODOMETRY) - ) + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + ) -def test_SwerveControllerIncompletePID( + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6PIDWithRotation( scheduler: commands2.CommandScheduler, get_swerve_controller_data ): with ManualSimTime() as sim: @@ -867,19 +1281,52 @@ def test_SwerveControllerIncompletePID( ) end_state = new_trajectory.sample(new_trajectory.totalTime()) - with pytest.raises(RuntimeError): - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(INCOMPLETE_PID_CLASSES) - ) + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data.setModuleStates, - subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, - desiredRotation=fixture_data.getRotationHeadingZero, - ) + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + subsystem, + xController=fixture_data._xController, + yController=fixture_data._yController, + thetaController=fixture_data._rotationController, + desiredRotation=fixture_data.getRotationHeadingZero, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + 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 + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) From 10c6705b5380c60fc54114c0cb0188d2dd5dee02 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Thu, 14 Dec 2023 10:08:28 -0500 Subject: [PATCH 08/14] Removed superfluous comment --- commands2/swervecontrollercommand.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index a4b180ec..09f8c1a2 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -35,8 +35,6 @@ class SwerveControllerCommand(Command): The robot angle controller does not follow the angle given by the trajectory but rather goes to the angle given in the final state of the trajectory. - - This class is provided by the NewCommands VendorDep """ @overload From f2828ae47b88cae4baa1ec1160e6a0f75a763d05 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 29 Dec 2023 20:16:18 -0500 Subject: [PATCH 09/14] Updated implementation to use Overtake and Beartype. --- commands2/swervecontrollercommand.py | 204 +++++++++++++------------- setup.py | 7 +- tests/test_swervecontrollercommand.py | 90 ++++++------ 3 files changed, 153 insertions(+), 148 deletions(-) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index 09f8c1a2..9787def5 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -2,8 +2,9 @@ # 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, Optional, Union, overload, Tuple - +from typing import Callable, Optional, Union, Tuple +from typing_extensions import overload +from overtake import overtake from wpimath.controller import ( HolonomicDriveController, PIDController, @@ -48,72 +49,13 @@ def __init__( SwerveDrive4Kinematics, SwerveDrive6Kinematics, ], - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - *, xController: PIDController, yController: PIDController, thetaController: ProfiledPIDControllerRadians, desiredRotation: Callable[[], Rotation2d], - ) -> None: - ... - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - *, - xController: PIDController, - yController: PIDController, - thetaController: ProfiledPIDControllerRadians, - ) -> None: - ... - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - *, - controller: HolonomicDriveController, - desiredRotation: Callable[[], Rotation2d], - ) -> None: - ... - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], outputModuleStates: Callable[[SwerveModuleState], None], requirements: Tuple[Subsystem], - *, - controller: HolonomicDriveController, ) -> None: - ... """ Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but @@ -129,7 +71,7 @@ def __init__( :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 SwerveKinematics. :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. + :param requirements: The subsystems to require. :param xController: The Trajectory Tracker PID controller for the robot's x position. :param yController: The Trajectory Tracker PID controller @@ -138,8 +80,36 @@ def __init__( for angle for the robot. :param desiredRotation: The angle that the drivetrain should be facing. This is sampled at each time step. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = HolonomicDriveController( + xController, yController, thetaController + ) + self._desiredRotation = desiredRotation + self._timer = Timer() - + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + xController: PIDController, + yController: PIDController, + thetaController: ProfiledPIDControllerRadians, + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but rather raw module states from the position controllers which need to be put @@ -159,15 +129,43 @@ def __init__( provide this. :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 SwerveKinematics. - :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. :param xController: The Trajectory Tracker PID controller for the robot's x position. :param yController: The Trajectory Tracker PID controller for the robot's y position. :param thetaController: The Trajectory Tracker PID controller for angle for the robot. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = HolonomicDriveController( + xController, yController, thetaController + ) + self._desiredRotation = self._trajectory.states()[-1].pose.rotation + self._timer = Timer() + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + controller: HolonomicDriveController, + desiredRotation: Callable[[], Rotation2d], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but rather raw module states from the position controllers which need to be put @@ -182,13 +180,38 @@ def __init__( provide this. :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 SwerveKinematics. - :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. :param controller: The HolonomicDriveController for the drivetrain. :param desiredRotation: The angle that the drivetrain should be facing. This is sampled at each time step. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = controller + self._desiredRotation = desiredRotation + self._timer = Timer() + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + controller: HolonomicDriveController, + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but rather raw module states from the position controllers which need to be put @@ -208,11 +231,21 @@ def __init__( provide this. :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 SwerveKinematics. + :param controller: The HolonomicDriveController for the drivetrain. :param outputModuleStates: The raw output module states from the position controllers. :param requirements: The subsystems to require. - :param controller: The HolonomicDriveController for the drivetrain. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = controller + self._desiredRotation = self._trajectory.states()[-1].pose.rotation + self._timer = Timer() + @overtake(runtime_type_checker="beartype") def __init__( self, trajectory: Trajectory, @@ -232,40 +265,7 @@ def __init__( thetaController: Optional[ProfiledPIDControllerRadians] = None, desiredRotation: Optional[Callable[[], Rotation2d]] = None, ): - super().__init__() - self._trajectory = trajectory - self._pose = pose - self._kinematics = kinematics - self._outputModuleStates = outputModuleStates - - # Parse the controller parameters combination. If the controller is passed in constructed, - # ignore the PID controllers. If it's None, parse them. - if controller is not None: - self._controller = controller - else: - # Verify the PID controller combination - if (xController and yController and thetaController) is None: - raise RuntimeError( - f"Failed to instantiate the Swerve2ControllerCommand: Could not create HolonomicDriveController from PID requirements" - ) - - # Adding the mypy type error annotation since it incorrectly is giving an error that the type is: - # PIDController | None expected PIDController - # The statement is true, but ignore the error because the None type check on above makes sure the - # typing is correct. - self._controller = HolonomicDriveController(xController, yController, thetaController) # type: ignore - - # If the desired rotation isn't provided, just take the final rotation from the trajectory - if desiredRotation is not None: - self._desiredRotation = desiredRotation - else: - self._desiredRotation = self._trajectory.states()[-1].pose.rotation - self.addRequirements( - # Ignoring type due to mypy stating the addRequirements was expecting a Subsystem but got an iterable of a subsystem. - # The iterable of the subsystem was to be able to force optional kwargs for the users when overloading the constructor. - requirements, # type: ignore - ) - self._timer = Timer() + ... def initialize(self): self._timer.restart() 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_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index b1c728d5..acb5f3b9 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -298,7 +298,7 @@ def test_SwerveControllerIncompletePID( ) end_state = new_trajectory.sample(new_trajectory.totalTime()) - with pytest.raises(RuntimeError): + with pytest.raises(Exception): fixture_data: SwerveControllerCommandTestDataFixtures = ( get_swerve_controller_data(INCOMPLETE_PID_CLASSES) ) @@ -308,11 +308,11 @@ def test_SwerveControllerIncompletePID( fixture_data.getRobotPose, fixture_data._kinematics, fixture_data.setModuleStates, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, - desiredRotation=fixture_data.getRotationHeadingZero, ) @@ -338,9 +338,9 @@ def test_SwerveControllerCommand2Holonomic( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, ) fixture_data._timer.restart() @@ -400,10 +400,10 @@ def test_SwerveControllerCommand2HolonomicWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -463,11 +463,11 @@ def test_SwerveControllerCommand2PID( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, ) fixture_data._timer.restart() @@ -527,12 +527,12 @@ def test_SwerveControllerCommand2PIDWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -592,9 +592,9 @@ def test_SwerveControllerCommand3Holonomic( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, ) fixture_data._timer.restart() @@ -654,10 +654,10 @@ def test_SwerveControllerCommand3HolonomicWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -717,11 +717,11 @@ def test_SwerveControllerCommand3PID( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, ) fixture_data._timer.restart() @@ -781,12 +781,12 @@ def test_SwerveControllerCommand3PIDWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -846,9 +846,9 @@ def test_SwerveControllerCommand4Holonomic( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, ) fixture_data._timer.restart() @@ -908,10 +908,10 @@ def test_SwerveControllerCommand4HolonomicWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -971,11 +971,11 @@ def test_SwerveControllerCommand4PID( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, ) fixture_data._timer.restart() @@ -1035,12 +1035,12 @@ def test_SwerveControllerCommand4PIDWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -1100,9 +1100,9 @@ def test_SwerveControllerCommand6Holonomic( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, ) fixture_data._timer.restart() @@ -1162,10 +1162,10 @@ def test_SwerveControllerCommand6HolonomicWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - controller=fixture_data._holonomic, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() @@ -1225,11 +1225,11 @@ def test_SwerveControllerCommand6PID( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, ) fixture_data._timer.restart() @@ -1289,12 +1289,12 @@ def test_SwerveControllerCommand6PIDWithRotation( new_trajectory, fixture_data.getRobotPose, fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, fixture_data.setModuleStates, subsystem, - xController=fixture_data._xController, - yController=fixture_data._yController, - thetaController=fixture_data._rotationController, - desiredRotation=fixture_data.getRotationHeadingZero, ) fixture_data._timer.restart() From 95b54b981b31147fd59beb7af633c7a4d5f5ea4b Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 29 Dec 2023 20:38:56 -0500 Subject: [PATCH 10/14] Fixed test fixtures to supply tuple to odometry update --- tests/test_swervecontrollercommand.py | 30 +-------------------------- 1 file changed, 1 insertion(+), 29 deletions(-) diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index acb5f3b9..b64e783d 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -220,35 +220,7 @@ def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: self._moduleStates = states def getRobotPose(self) -> geometry.Pose2d: - if isinstance(self._odometry, kinematics.SwerveDrive2Odometry): - self._odometry.update( - self._angle, self._modulePositions[0], self._modulePositions[1] - ) - elif isinstance(self._odometry, kinematics.SwerveDrive3Odometry): - self._odometry.update( - self._angle, - self._modulePositions[0], - self._modulePositions[1], - self._modulePositions[2], - ) - elif isinstance(self._odometry, kinematics.SwerveDrive4Odometry): - self._odometry.update( - self._angle, - self._modulePositions[0], - self._modulePositions[1], - self._modulePositions[2], - self._modulePositions[3], - ) - elif isinstance(self._odometry, kinematics.SwerveDrive6Odometry): - self._odometry.update( - self._angle, - self._modulePositions[0], - self._modulePositions[1], - self._modulePositions[2], - self._modulePositions[3], - self._modulePositions[4], - self._modulePositions[5], - ) + self._odometry.update(self._angle, *self._modulePositions) return self._odometry.getPose() From 3a441f4a7b8b5b5d70ea275e5819f4237a0648d5 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Fri, 29 Dec 2023 21:09:51 -0500 Subject: [PATCH 11/14] Fixed tests for odometry update calls to send Tuples --- tests/test_swervecontrollercommand.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index b64e783d..a8f6cf1a 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -41,8 +41,8 @@ def __init__(self, selector: int): # The module positions and states start empty and will be populated below in the selector # self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] - self._modulePositions: List[kinematics.SwerveModulePosition] = [] - self._moduleStates: List[kinematics.SwerveModuleState] = [] + self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] + self._moduleStates: Tuple[kinematics.SwerveModuleState] = [] # Setup PID controllers, but if an error test case is requested, make sure it provides # data that should break the command instantiation From 0efaf1d2de05e2ef0e1b6e6f37fabef32b3e04cd Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Sat, 30 Dec 2023 00:28:57 -0500 Subject: [PATCH 12/14] Fix argument --- tests/test_swervecontrollercommand.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py index a8f6cf1a..b94a47f7 100644 --- a/tests/test_swervecontrollercommand.py +++ b/tests/test_swervecontrollercommand.py @@ -220,7 +220,7 @@ def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: self._moduleStates = states def getRobotPose(self) -> geometry.Pose2d: - self._odometry.update(self._angle, *self._modulePositions) + self._odometry.update(self._angle, self._modulePositions) return self._odometry.getPose() From 68f1f5def050dcf7492e847e01f4bbc321522e7e Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Sat, 30 Dec 2023 14:30:09 -0500 Subject: [PATCH 13/14] Bug fix: Added addRequirements call to contstructors --- commands2/swervecontrollercommand.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index 9787def5..6d438f4e 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -91,6 +91,7 @@ def __init__( ) self._desiredRotation = desiredRotation self._timer = Timer() + self.addRequirements(requirements) # type: ignore @overload def __init__( @@ -148,6 +149,7 @@ def __init__( ) self._desiredRotation = self._trajectory.states()[-1].pose.rotation self._timer = Timer() + self.addRequirements(requirements) # type:ignore @overload def __init__( @@ -195,6 +197,7 @@ def __init__( self._controller = controller self._desiredRotation = desiredRotation self._timer = Timer() + self.addRequirements(requirements) # type:ignore @overload def __init__( @@ -244,6 +247,7 @@ def __init__( self._controller = controller self._desiredRotation = self._trajectory.states()[-1].pose.rotation self._timer = Timer() + self.addRequirements(requirements) # type: ignore @overtake(runtime_type_checker="beartype") def __init__( From bb2778c242e08d3cf9dc4f6d894c46cd29103f30 Mon Sep 17 00:00:00 2001 From: NewtonCrosby Date: Sat, 30 Dec 2023 14:43:08 -0500 Subject: [PATCH 14/14] Pinned versions of experimental libraries --- setup.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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",