[wpimath] Add ChassisSpeeds.fromRobotRelativeSpeeds() (#5744)

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.
    *

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.
    *

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);

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 <cmath>
+
 #include <gtest/gtest.h>
 
 #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};