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 5d36440233..662714ad85 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 @@ -6,6 +6,7 @@ package edu.wpi.first.math.kinematics; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; /** * Represents the speed of a robot chassis. Although this class contains similar members compared to @@ -116,10 +117,10 @@ public class ChassisSpeeds { double vyMetersPerSecond, double omegaRadiansPerSecond, Rotation2d robotAngle) { - return new ChassisSpeeds( - vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(), - -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(), - omegaRadiansPerSecond); + // CW rotation into chassis frame + var rotated = + new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus()); + return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); } /** @@ -143,6 +144,51 @@ public class ChassisSpeeds { robotAngle); } + /** + * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds + * object. + * + * @param vxMetersPerSecond The component of speed in the x direction relative to the robot. + * Positive x is towards the robot's front. + * @param vyMetersPerSecond The component of speed in the y direction relative to the robot. + * Positive y is towards the robot's left. + * @param omegaRadiansPerSecond The angular rate of the robot. + * @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. + * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + */ + public static ChassisSpeeds fromRobotRelativeSpeeds( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond, + Rotation2d robotAngle) { + // CCW rotation out of chassis frame + var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle); + return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); + } + + /** + * Converts a user provided robot-relative ChassisSpeeds object into a field-relative + * ChassisSpeeds object. + * + * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame + * of reference. Positive x is towards the robot's front. Positive y is towards the robot's + * left. + * @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. + * @return ChassisSpeeds object representing the speeds in the field's frame of reference. + */ + public static ChassisSpeeds fromRobotRelativeSpeeds( + ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) { + return fromRobotRelativeSpeeds( + robotRelativeSpeeds.vxMetersPerSecond, + robotRelativeSpeeds.vyMetersPerSecond, + robotRelativeSpeeds.omegaRadiansPerSecond, + robotAngle); + } + /** * Adds two ChassisSpeeds and returns the sum. * diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index bfee34ea94..7b23a29cfc 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -108,8 +108,12 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { static ChassisSpeeds FromFieldRelativeSpeeds( units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, const Rotation2d& robotAngle) { - return {vx * robotAngle.Cos() + vy * robotAngle.Sin(), - -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega}; + // CW rotation into chassis frame + auto rotated = + Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}} + .RotateBy(-robotAngle); + return {units::meters_per_second_t{rotated.X().value()}, + units::meters_per_second_t{rotated.Y().value()}, omega}; } /** @@ -133,6 +137,54 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { fieldRelativeSpeeds.omega, robotAngle); } + /** + * Converts a user provided robot-relative set of speeds into a field-relative + * ChassisSpeeds object. + * + * @param vx The component of speed in the x direction relative to the robot. + * Positive x is towards the robot's front. + * @param vy The component of speed in the y direction relative to the robot. + * Positive y is towards the robot's left. + * @param omega The angular rate of the robot. + * @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. + * + * @return ChassisSpeeds object representing the speeds in the field's frame + * of reference. + */ + static ChassisSpeeds FromRobotRelativeSpeeds( + units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, const Rotation2d& robotAngle) { + // CCW rotation out of chassis frame + auto rotated = + Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}} + .RotateBy(robotAngle); + return {units::meters_per_second_t{rotated.X().value()}, + units::meters_per_second_t{rotated.Y().value()}, omega}; + } + + /** + * Converts a user provided robot-relative ChassisSpeeds object into a + * field-relative ChassisSpeeds object. + * + * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds + * in the robot frame of reference. Positive x is the towards robot's + * front. Positive y is towards the robot's left. + * @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. + * @return ChassisSpeeds object representing the speeds in the field's frame + * of reference. + */ + static ChassisSpeeds FromRobotRelativeSpeeds( + const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) { + return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx, + robotRelativeSpeeds.vy, + robotRelativeSpeeds.omega, robotAngle); + } + /** * 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 034bebda21..24013427bf 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 @@ -55,6 +55,17 @@ class ChassisSpeedsTest { () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); } + @Test + void testFromRobotRelativeSpeeds() { + final var chassisSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0)); + + assertAll( + () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon), + () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon), + () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + } + @Test void testPlus() { final var left = new ChassisSpeeds(1.0, 0.5, 0.75); diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp index bab3ee558e..97de03bcf6 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp @@ -2,6 +2,8 @@ // 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. +#include + #include #include "frc/kinematics/ChassisSpeeds.h" @@ -36,6 +38,15 @@ TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) { EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon); } +TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) { + const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds( + 1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg); + + EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon); + EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon); + EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon); +} + TEST(ChassisSpeedsTest, Plus) { const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s}; const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};