diff --git a/commands2/__init__.py b/commands2/__init__.py index 4d9c2e8c..52efd58b 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -42,6 +42,7 @@ from .sequentialcommandgroup import SequentialCommandGroup from .startendcommand import StartEndCommand from .subsystem import Subsystem +from .swervecontrollercommand import SwerveControllerCommand from .timedcommandrobot import TimedCommandRobot from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem from .waitcommand import WaitCommand @@ -75,6 +76,7 @@ "SequentialCommandGroup", "StartEndCommand", "Subsystem", + "SwerveControllerCommand", "TimedCommandRobot", "TrapezoidProfileSubsystem", "WaitCommand", diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py new file mode 100644 index 00000000..6d438f4e --- /dev/null +++ b/commands2/swervecontrollercommand.py @@ -0,0 +1,292 @@ +# 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, Tuple +from typing_extensions import overload +from overtake import overtake +from wpimath.controller import ( + HolonomicDriveController, + PIDController, + ProfiledPIDControllerRadians, +) +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. + """ + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + xController: PIDController, + yController: PIDController, + thetaController: ProfiledPIDControllerRadians, + 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 + 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. + """ + 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() + self.addRequirements(requirements) # type: ignore + + @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 + 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 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() + self.addRequirements(requirements) # type:ignore + + @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 + 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 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() + self.addRequirements(requirements) # type:ignore + + @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 + 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 controller: The HolonomicDriveController for the drivetrain. + :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 = self._trajectory.states()[-1].pose.rotation + self._timer = Timer() + self.addRequirements(requirements) # type: ignore + + @overtake(runtime_type_checker="beartype") + 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, + ): + ... + + 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/setup.py b/setup.py index b1c069cb..99b6e032 100644 --- a/setup.py +++ b/setup.py @@ -11,7 +11,12 @@ description="WPILib command framework v2", url="https://github.com/robotpy/robotpy-commands-v2", packages=["commands2"], - install_requires=["wpilib<2025,>=2024.0.0b2", "typing_extensions>=4.1.0,<5"], + install_requires=[ + "wpilib<2025,>=2024.0.0b2", + "typing_extensions>=4.1.0,<5", + "overtake~=0.4.0", + "beartype~=0.16.4", + ], license="BSD-3-Clause", python_requires=">=3.8", include_package_data=True, diff --git a/tests/test_swervecontrollercommand.py b/tests/test_swervecontrollercommand.py new file mode 100644 index 00000000..b94a47f7 --- /dev/null +++ b/tests/test_swervecontrollercommand.py @@ -0,0 +1,1304 @@ +# 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: 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 + 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._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) 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), + ) + + 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)), + ) + + def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: + self._moduleStates = states + + def getRobotPose(self) -> geometry.Pose2d: + self._odometry.update(self._angle, self._modulePositions) + + 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_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(Exception): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(INCOMPLETE_PID_CLASSES) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + subsystem, + ) + + +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._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + 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_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._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + 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_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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + 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_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._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + 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_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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + 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_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._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + 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_SwerveControllerCommand4PIDWithRotation( + 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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + 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_SwerveControllerCommand6HolonomicWithRotation( + 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._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + 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: + 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._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + 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, + )