diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index e00abf6d302..addedc0a9a7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -136,14 +136,12 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var mecanumDriveWheelSpeeds = - m_kinematics.toWheelSpeeds( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d()); + } + chassisSpeeds.discretize(periodSeconds); + var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); setSpeeds(mecanumDriveWheelSpeeds); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index a660c2b07ba..1143278b3a0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -148,14 +148,12 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var mecanumDriveWheelSpeeds = - m_kinematics.toWheelSpeeds( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation()); + } + chassisSpeeds.discretize(periodSeconds); + var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); setSpeeds(mecanumDriveWheelSpeeds); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 958e5a3e617..f622259b494 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -57,14 +57,12 @@ public Drivetrain() { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var swerveModuleStates = - m_kinematics.toSwerveModuleStates( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d()); + } + chassisSpeeds.discretize(periodSeconds); + var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 04ee2998df6..c950e4c7c53 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -118,14 +118,12 @@ public void resetOdometry(Pose2d pose) { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - var swerveModuleStates = - DriveConstants.kDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - DriveConstants.kDrivePeriod)); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d()); + } + chassisSpeeds.discretize(DriveConstants.kDrivePeriod); + var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); m_frontLeft.setDesiredState(swerveModuleStates[0]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index 9d68c0e3c34..bccc93de7b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -36,8 +36,11 @@ public class Drivetrain { new SwerveDriveKinematics( m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); - /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used - below are robot specific, and should be tuned. */ + /* + * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. + * The numbers used + * below are robot specific, and should be tuned. + */ private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( m_kinematics, @@ -66,14 +69,12 @@ public Drivetrain() { */ public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { - var swerveModuleStates = - m_kinematics.toSwerveModuleStates( - ChassisSpeeds.discretize( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) - : new ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds)); + var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + if (fieldRelative) { + chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation()); + } + chassisSpeeds.discretize(periodSeconds); + var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); @@ -92,7 +93,8 @@ public void updateOdometry() { m_backRight.getPosition() }); - // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on + // Also apply vision measurements. We use 0.3 seconds in the past as an example + // -- on // a real robot, this must be calculated based either on latency or timestamps. m_poseEstimator.addVisionMeasurement( ExampleGlobalMeasurementSensor.getEstimatedGlobalPose( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index 7775a30de78..e1c6c37f50a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -102,9 +102,10 @@ public ChassisSpeeds calculate( m_poseError = trajectoryPose.relativeTo(currentPose); m_rotationError = desiredHeading.minus(currentPose.getRotation()); - + ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF); if (!m_enabled) { - return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation()); + speeds.toRobotRelativeSpeeds(currentPose.getRotation()); + return speeds; } // Calculate feedback velocities (based on position error). @@ -112,8 +113,10 @@ public ChassisSpeeds calculate( double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY()); // Return next output. - return ChassisSpeeds.fromFieldRelativeSpeeds( - xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation()); + speeds.vxMetersPerSecond += xFeedback; + speeds.vyMetersPerSecond += yFeedback; + speeds.toRobotRelativeSpeeds(currentPose.getRotation()); + return speeds; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 4c806a52cb7..e0eb4077961 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -103,7 +103,9 @@ public Twist2d toTwist2d(double dtSeconds) { * @param omegaRadiansPerSecond Angular velocity. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. + * @deprecated Use instance method instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds discretize( double vxMetersPerSecond, double vyMetersPerSecond, @@ -141,7 +143,9 @@ public static ChassisSpeeds discretize( * @param omega Angular velocity. * @param dt The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. + * @deprecated Use instance method instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds discretize( LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) { return discretize( @@ -162,7 +166,9 @@ public static ChassisSpeeds discretize( * @param continuousSpeeds The continuous speeds. * @param dtSeconds The duration of the timestep the speeds should be applied for. * @return Discretized ChassisSpeeds. + * @deprecated Use instance method instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) { return discretize( continuousSpeeds.vxMetersPerSecond, @@ -171,6 +177,36 @@ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt dtSeconds); } + /** + * Discretizes a continuous-time chassis speed. + * + *

This function converts this continuous-time chassis speed into a discrete-time one such that + * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the + * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt + * along the y-axis, and omega * dt around the z-axis). + * + *

This is useful for compensating for translational skew when translating and rotating a + * swerve drivetrain. + * + * @param dtSeconds The duration of the timestep the speeds should be applied for. + */ + public void discretize(double dtSeconds) { + var desiredDeltaPose = + new Pose2d( + vxMetersPerSecond * dtSeconds, + vyMetersPerSecond * dtSeconds, + new Rotation2d(omegaRadiansPerSecond * dtSeconds)); + + // Find the chassis translation/rotation deltas in the robot frame that move the robot from its + // current pose to the desired pose + var twist = Pose2d.kZero.log(desiredDeltaPose); + + // Turn the chassis translation/rotation deltas into average velocities + vxMetersPerSecond = twist.dx / dtSeconds; + vyMetersPerSecond = twist.dy / dtSeconds; + omegaRadiansPerSecond = twist.dtheta / dtSeconds; + } + /** * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds * object. @@ -184,7 +220,9 @@ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + * @deprecated Use toRobotRelativeSpeeds instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromFieldRelativeSpeeds( double vxMetersPerSecond, double vyMetersPerSecond, @@ -209,7 +247,9 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + * @deprecated Use toRobotRelativeSpeeds instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromFieldRelativeSpeeds( LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) { return fromFieldRelativeSpeeds( @@ -227,7 +267,9 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + * @deprecated Use toRobotRelativeSpeeds instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromFieldRelativeSpeeds( ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) { return fromFieldRelativeSpeeds( @@ -237,6 +279,21 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( robotAngle); } + /** + * Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object. + * + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + */ + public void toRobotRelativeSpeeds(Rotation2d robotAngle) { + // CW rotation into chassis frame + var rotated = + new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus()); + vxMetersPerSecond = rotated.getX(); + vyMetersPerSecond = rotated.getY(); + } + /** * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds * object. @@ -250,7 +307,9 @@ public static ChassisSpeeds fromFieldRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + * @deprecated Use toFieldRelativeSpeeds instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromRobotRelativeSpeeds( double vxMetersPerSecond, double vyMetersPerSecond, @@ -274,7 +333,9 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + * @deprecated Use toFieldRelativeSpeeds instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromRobotRelativeSpeeds( LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) { return fromRobotRelativeSpeeds( @@ -292,7 +353,9 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( * considered to be zero when it is facing directly away from your alliance station wall. * Remember that this should be CCW positive. * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + * @deprecated Use toFieldRelativeSpeeds instead. */ + @Deprecated(forRemoval = true, since = "2025") public static ChassisSpeeds fromRobotRelativeSpeeds( ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) { return fromRobotRelativeSpeeds( @@ -302,6 +365,20 @@ public static ChassisSpeeds fromRobotRelativeSpeeds( robotAngle); } + /** + * Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object. + * + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + */ + public void toFieldRelativeSpeeds(Rotation2d robotAngle) { + // CCW rotation out of chassis frame + var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle); + vxMetersPerSecond = rotated.getX(); + vyMetersPerSecond = rotated.getY(); + } + /** * Adds two ChassisSpeeds and returns the sum. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 8f7bb6fd123..5ed2e7f22a1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -20,10 +20,11 @@ class ChassisSpeedsTest { @Test void testDiscretize() { final var target = new ChassisSpeeds(1.0, 0.0, 0.5); + final var speeds = new ChassisSpeeds(1.0, 0.0, 0.5); final var duration = 1.0; final var dt = 0.01; - final var speeds = ChassisSpeeds.discretize(target, duration); + speeds.discretize(duration); final var twist = new Twist2d( speeds.vxMetersPerSecond * dt, @@ -61,8 +62,8 @@ void testMeasureConstructor() { @Test void testFromFieldRelativeSpeeds() { - final var chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2); + final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5); + chassisSpeeds.toRobotRelativeSpeeds(Rotation2d.kCW_Pi_2); assertAll( () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), @@ -72,8 +73,8 @@ void testFromFieldRelativeSpeeds() { @Test void testFromRobotRelativeSpeeds() { - final var chassisSpeeds = - ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0)); + final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5); + chassisSpeeds.toFieldRelativeSpeeds(Rotation2d.fromDegrees(45.0)); assertAll( () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),