diff --git a/commands2/__init__.py b/commands2/__init__.py index 4d9c2e8c..bb18e552 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -35,6 +35,7 @@ from .printcommand import PrintCommand from .proxycommand import ProxyCommand from .proxyschedulecommand import ProxyScheduleCommand +from .ramsetecommand import RamseteCommand from .repeatcommand import RepeatCommand from .runcommand import RunCommand from .schedulecommand import ScheduleCommand @@ -68,6 +69,7 @@ "PrintCommand", "ProxyCommand", "ProxyScheduleCommand", + "RamseteCommand", "RepeatCommand", "RunCommand", "ScheduleCommand", diff --git a/commands2/ramsetecommand.py b/commands2/ramsetecommand.py new file mode 100644 index 00000000..922b6a7f --- /dev/null +++ b/commands2/ramsetecommand.py @@ -0,0 +1,229 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +from __future__ import annotations + +from typing import Callable, Union, Optional, Tuple +from typing_extensions import overload +from overtake import overtake +from wpimath.controller import ( + PIDController, + RamseteController, + SimpleMotorFeedforwardMeters, +) +from wpimath.geometry import Pose2d +from wpimath.kinematics import ( + ChassisSpeeds, + DifferentialDriveKinematics, + DifferentialDriveWheelSpeeds, +) +from wpimath.trajectory import Trajectory +from wpimath import units as units +from wpiutil import SendableBuilder +from wpilib import Timer + + +from .command import Command +from .subsystem import Subsystem + + +class RamseteCommand(Command): + """ + A command that uses a RAMSETE controller (RamseteController) to follow a trajectory + (Trajectory) with a differential drive. + + The command handles trajectory-following, PID calculations, and feedforwards internally. This + is intended to be a more-or-less "complete solution" that can be used by teams without a great + deal of controls expertise. + + Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID + functionality of a "smart" motor controller) may use the secondary constructor that omits the PID + and feedforward functionality, returning only the raw wheel speeds from the RAMSETE controller. + """ + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + controller: RamseteController, + kinematics: DifferentialDriveKinematics, + requirements: Tuple[Subsystem], + output: Callable[[float, float], None], + ): + """Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID + control and feedforward are handled internally, and outputs are scaled -12 to 12 representing + units of volts. + + Note: The controller will *not* set the outputVolts to zero upon completion of the path - + this is left to the user, since it is not appropriate for paths with nonstationary endstates. + + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param controller: The RAMSETE controller used to follow the trajectory. + :param kinematics: The kinematics for the robot drivetrain. + :param requirements: The subsystems to require. + :param output A function that consumes the computed left and right outputs in `units.meters_per_second` + """ + super().__init__() + + self._timer = Timer() + self._trajectory = trajectory + self._pose = pose + self._follower = controller + self._kinematics = kinematics + self._output = output + self._usePID = False + # All the parameter checks pass, inform scheduler. Type ignoring is set explictitly for the linter because this + # implementation declares the tuple explicitly, whereas the general implementations use the unpack operator (*) + # to pass the requirements to the scheduler. + self.addRequirements(requirements) # type: ignore + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + controller: RamseteController, + kinematics: DifferentialDriveKinematics, + requirements: Tuple[Subsystem], + output: Callable[[float, float], None], + feedforward: SimpleMotorFeedforwardMeters, + leftController: PIDController, + rightController: PIDController, + wheelSpeeds: Callable[[], DifferentialDriveWheelSpeeds], + ): + """Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID + control and feedforward are handled internally, and outputs are scaled -12 to 12 representing + units of volts. + + Note: The controller will *not* set the outputVolts to zero upon completion of the path - + this is left to the user, since it is not appropriate for paths with nonstationary endstates. + + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param controller: The RAMSETE controller used to follow the trajectory. + :param kinematics: The kinematics for the robot drivetrain. + :param requirements: The subsystems to require. + :param output A function that consumes the computed left and right outputs in `units.volts` + :param feedforward The feedforward to use for the drive + :param leftController: The PIDController for the left side of the robot drive. + :param rightController: The PIDController for the right side of the robot drive. + :param wheelSpeeds: A function that supplies the speeds of the left and right sides of the robot + drive. + """ + super().__init__() + + self._timer = Timer() + + # Store all the requirements that are required + self._trajectory = trajectory + self._pose = pose + self._follower = controller + self._kinematics = kinematics + self._output = output + self._leftController = leftController + self._rightController = rightController + self._wheelspeeds = wheelSpeeds + self._feedforward = feedforward + self._prevSpeeds = DifferentialDriveWheelSpeeds() + self._prevTime = -1.0 + self._usePID = True + # All the parameter checks pass, inform scheduler. Type ignoring is set explictitly for the linter because this + # implementation declares the tuple explicitly, whereas the general implementations use the unpack operator (*) + # to pass the requirements to the scheduler. + self.addRequirements(requirements) # type: ignore + + @overtake(runtime_type_checker="beartype") + def __init__( + self, + *, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + controller: RamseteController, + kinematics: DifferentialDriveKinematics, + requirements: Tuple[Subsystem], + output: Callable[[float, float], None], + feedforward: Optional[SimpleMotorFeedforwardMeters] = None, + leftController: Optional[PIDController] = None, + rightController: Optional[PIDController] = None, + wheelSpeeds: Optional[Callable[[], DifferentialDriveWheelSpeeds]] = None, + ): + ... + + def initialize(self): + self._prevTime = -1 + initial_state = self._trajectory.sample(0) + self._prevSpeeds = self._kinematics.toWheelSpeeds( + ChassisSpeeds( + initial_state.velocity, + 0, + initial_state.curvature * initial_state.velocity, + ) + ) + self._timer.restart() + if self._usePID: + self._leftController.reset() + self._rightController.reset() + + def execute(self): + curTime = self._timer.get() + dt = curTime - self._prevTime + + if self._prevTime < 0: + self._output(0.0, 0.0) + self._prevTime = curTime + return + + targetWheelSpeeds = self._kinematics.toWheelSpeeds( + self._follower.calculate(self._pose(), self._trajectory.sample(curTime)) + ) + + leftSpeedSetpoint = targetWheelSpeeds.left + rightSpeedSetpoint = targetWheelSpeeds.right + + if self._usePID: + leftFeedforward = self._feedforward.calculate( + leftSpeedSetpoint, (leftSpeedSetpoint - self._prevSpeeds.left) / dt + ) + + rightFeedforward = self._feedforward.calculate( + rightSpeedSetpoint, + (rightSpeedSetpoint - self._prevSpeeds.right) / dt, + ) + + leftOutput = leftFeedforward + self._leftController.calculate( + self._wheelspeeds().left, leftSpeedSetpoint + ) + + rightOutput = rightFeedforward + self._rightController.calculate( + self._wheelspeeds().right, rightSpeedSetpoint + ) + self._output(leftOutput, rightOutput) + else: + leftOutput = leftSpeedSetpoint + rightOutput = rightSpeedSetpoint + self._output(leftOutput, rightOutput) + + self._prevSpeeds = targetWheelSpeeds + self._prevTime = curTime + + def end(self, interrupted: bool): + self._timer.stop() + + if interrupted: + self._output(0.0, 0.0) + + def isFinished(self): + return self._timer.hasElapsed(self._trajectory.totalTime()) + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + builder.addDoubleProperty( + "leftVelocity", lambda: self._prevSpeeds.left, lambda *float: None + ) + builder.addDoubleProperty( + "rightVelocity", lambda: self._prevSpeeds.right, lambda *float: None + ) diff --git a/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_ramsetecommand.py b/tests/test_ramsetecommand.py new file mode 100644 index 00000000..604380d5 --- /dev/null +++ b/tests/test_ramsetecommand.py @@ -0,0 +1,424 @@ +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. + +from typing import TYPE_CHECKING, List +import math + +import wpimath.controller as controller +import wpimath.trajectory as trajectory +import wpimath.trajectory.constraint as constraints +import wpimath.geometry as geometry +import wpimath.kinematics as kinematics +import wpimath.units as units +from wpilib import Timer + +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + +import commands2 + + +class RamseteCommandTestDataFixtures: + def __init__(self): + self.timer = Timer() + self.angle: geometry.Rotation2d = geometry.Rotation2d(0) + + # Track speeds and distances + self.leftSpeed = 0 + self.leftDistance = 0 + self.rightSpeed = 0 + self.rightDistance = 0 + + # Chassis/Drivetrain constants + self.kxTolerance = 6.0 / 12.0 + self.kyTolerance = 6.0 / 12.0 + self.kWheelBase = 0.5 + self.kTrackWidth = 0.5 + self.kWheelDiameterMeters = 0.1524 + self.kRamseteB = 2.0 + self.kRamseteZeta = 0.7 + self.ksVolts = 0.22 + self.kvVoltSecondsPerMeter = 1.98 + self.kaVoltSecondsSquaredPerMeter = 0.2 + self.kPDriveVel = 8.5 + self.kMaxMetersPerSecond = 3.0 + self.kMaxAccelerationMetersPerSecondSquared = 1.0 + self.kEncoderCPR = 1024 + self.kEncoderDistancePerPulse = ( + self.kWheelDiameterMeters * math.pi + ) / self.kEncoderCPR + + self.command_kinematics: kinematics.DifferentialDriveKinematics = ( + kinematics.DifferentialDriveKinematics(self.kTrackWidth) + ) + + self.command_voltage_constraint: constraints.DifferentialDriveVoltageConstraint = constraints.DifferentialDriveVoltageConstraint( + controller.SimpleMotorFeedforwardMeters( + self.ksVolts, + self.kvVoltSecondsPerMeter, + self.kaVoltSecondsSquaredPerMeter, + ), + self.command_kinematics, + 10, + ) + + self.command_odometry: kinematics.DifferentialDriveOdometry = ( + kinematics.DifferentialDriveOdometry( + self.angle, + self.leftDistance, + self.rightDistance, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + ) + + def setWheelSpeedsMPS( + self, leftspeed: units.meters_per_second, rightspeed: units.meters_per_second + ) -> None: + self.leftSpeed = leftspeed + self.rightSpeed = rightspeed + + def setWheelSpeedsVolts( + self, leftVolts: units.volts, rightVolts: units.volts + ) -> None: + self.leftSpeed = leftVolts + self.rightSpeed = rightVolts + + def getCurrentWheelDistances(self) -> kinematics.DifferentialDriveWheelPositions: + positions = kinematics.DifferentialDriveWheelPositions() + positions.right = self.rightDistance + positions.left = self.leftDistance + + return positions + + def getRobotPose(self) -> geometry.Pose2d: + positions = self.getCurrentWheelDistances() + self.command_odometry.update(self.angle, positions.left, positions.right) + return self.command_odometry.getPose() + + def getWheelSpeeds(self) -> kinematics.DifferentialDriveWheelSpeeds: + return kinematics.DifferentialDriveWheelSpeeds(self.leftSpeed, self.rightSpeed) + + +@pytest.fixture() +def get_ramsete_command_data() -> RamseteCommandTestDataFixtures: + return RamseteCommandTestDataFixtures() + + +def test_rameseteRaisesNoOutputRaises( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(Exception): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + ) + + +def test_rameseteRaisesOnlyFeedForward( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + feedforward_var: controller.SimpleMotorFeedforwardMeters = ( + controller.SimpleMotorFeedforwardMeters( + fixture_data.ksVolts, + fixture_data.kvVoltSecondsPerMeter, + fixture_data.kaVoltSecondsSquaredPerMeter, + ) + ) + + with pytest.raises(Exception): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + fixture_data.setWheelSpeedsMPS, + feedforward_var, + ) + + +def test_rameseteRaisesFeedForwardAndLeft( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + feedforward_var: controller.SimpleMotorFeedforwardMeters = ( + controller.SimpleMotorFeedforwardMeters( + fixture_data.ksVolts, + fixture_data.kvVoltSecondsPerMeter, + fixture_data.kaVoltSecondsSquaredPerMeter, + ) + ) + left_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) + + with pytest.raises(Exception): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + fixture_data.setWheelSpeedsMPS, + feedforward_var, + left_pid, + ) + + +def test_rameseteRaisesFeedForwardRightAndLeft( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + feedforward_var: controller.SimpleMotorFeedforwardMeters = ( + controller.SimpleMotorFeedforwardMeters( + fixture_data.ksVolts, + fixture_data.kvVoltSecondsPerMeter, + fixture_data.kaVoltSecondsSquaredPerMeter, + ) + ) + left_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) + right_pid: controller.PIDController = controller.PIDController(0.1, 0, 0) + + with pytest.raises(Exception): + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + fixture_data.setWheelSpeedsMPS, + feedforward_var, + left_pid, + right_pid, + ) + + +def test_ramseteCommandMPSReachesDestination( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + fixture_data.setWheelSpeedsMPS, + ) + + fixture_data.timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + + fixture_data.leftDistance += fixture_data.leftSpeed * 0.005 + fixture_data.rightDistance += fixture_data.rightSpeed * 0.005 + + sim.step(0.005) + + fixture_data.timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data.kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data.kyTolerance + ) + + +def test_ramseteCommandVoltsReachesDestination( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + fixture_data.setWheelSpeedsVolts, + ) + + fixture_data.timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + + fixture_data.leftDistance += fixture_data.leftSpeed * 0.005 + fixture_data.rightDistance += fixture_data.rightSpeed * 0.005 + + sim.step(0.005) + + fixture_data.timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data.kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data.kyTolerance + ) + + +def test_ramseteCommandPIDReachesDestination( + scheduler: commands2.CommandScheduler, get_ramsete_command_data +): + with ManualSimTime() as sim: + fixture_data = get_ramsete_command_data + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(3, 0, geometry.Rotation2d(0))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + traj_config.setKinematics(fixture_data.command_kinematics) + traj_config.addConstraint(fixture_data.command_voltage_constraint) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + feedforward_var: controller.SimpleMotorFeedforwardMeters = ( + controller.SimpleMotorFeedforwardMeters( + fixture_data.ksVolts, + fixture_data.kvVoltSecondsPerMeter, + fixture_data.kaVoltSecondsSquaredPerMeter, + ) + ) + left_pid: controller.PIDController = controller.PIDController(0.001, 0, 0) + rightt_pid: controller.PIDController = controller.PIDController(0.001, 0, 0) + + command = commands2.RamseteCommand( + new_trajectory, + fixture_data.getRobotPose, + controller.RamseteController( + fixture_data.kRamseteB, fixture_data.kRamseteZeta + ), + fixture_data.command_kinematics, + subsystem, + fixture_data.setWheelSpeedsVolts, + feedforward_var, + left_pid, + rightt_pid, + fixture_data.getWheelSpeeds, + ) + + fixture_data.timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + + fixture_data.leftDistance += fixture_data.leftSpeed * 0.005 + fixture_data.rightDistance += fixture_data.rightSpeed * 0.005 + + sim.step(0.005) + + fixture_data.timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data.kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data.kyTolerance + )