Improved Euclidean to Wheel Readability / Logic? + Fixed Tests

Author: sunghyuneun

src/software/physics/euclidean_to_wheel.cpp

@@ -8,39 +8,40 @@
 #include "software/geom/angular_velocity.h"
 #include "software/geom/vector.h"
 
-#ifdef DEBUG_WHEEL
+// get rid of this stupid ifdef, and then look at the issue 
+
+// Turn this to 
+#if CHECK_VERSION(2026)
 EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants)
     : robot_constants_(robot_constants)
 {
     // Angles [rads]
     auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
     auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;
 
-    auto cos_p = std::cos(p);
-    auto sin_p = std::sin(p);
-    auto cos_t = std::cos(t);
-    auto sin_t = std::sin(t);
+    // Beta is angle of wheel rolling direction relative to X-axis
+    // LOOK AT (software)/src/shared/robot_constants.h to see X-axis
+    double b_fr = -(M_PI - p);
+    double b_fl = -p;
+    double b_bl = t;
+    double b_br = (M_PI - t);
 
-    // 1. Physical wheel coordinates in meters
-    // Mapped to the robot frame: +X = Right, +Y = Forward
-    double fr_x = 0.06632, fr_y = 0.03485;
-    double br_x = 0.05592, br_y = -0.04985;
+    // Mapped to the robot frame: +X = Forward, +Y = Left
+    double fr_x =  0.03485, fr_y = -0.06632;
+    double fl_x =  0.03485, fl_y =  0.06632;
+    double bl_x = -0.04985, bl_y =  0.05592;
+    double br_x = -0.04985, br_y = -0.05592;
 
-    // 2. Unit drive vectors (extracted directly from the matrix's linear columns)
-    double fr_dx = -sin_p, fr_dy = cos_p;
-    double br_dx = sin_t, br_dy = cos_t;
 
-    // 3. Dynamically calculate the rotational lever arms using the 2D cross product
-    // Lever Arm = | X * Dy - Y * Dx |
-    double rot_f = std::abs((fr_x * fr_dy) - (fr_y * fr_dx));
-    double rot_b = std::abs((br_x * br_dy) - (br_y * br_dx));
+    // Assuming that CCW when looking end of shaft into motor is the positive direction.
+    // Formula is u_1 = v_x * cos(B_1) + v_y * sin(B_1) + W * (x_1 * sin(B_1) - y_1 * cos(B_1))
 
     // clang-format off
     euclidean_to_wheel_velocity_D_ <<
-        -sin_p,  cos_p, rot_f,
-        -sin_p, -cos_p, rot_f,
-         sin_t, -cos_t, rot_b,
-         sin_t,  cos_t, rot_b;
+         std::cos(b_fr), std::sin(b_fr), (fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)),
+         std::cos(b_fl), std::sin(b_fl), (fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)),
+         std::cos(b_bl), std::sin(b_bl), (bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)),
+         std::cos(b_br), std::sin(b_br), (br_x * std::sin(b_br) - br_y * std::cos(b_br));
     // clang-format on
 
     // Calculate Pseudo-inverse dynamically
@@ -102,7 +103,8 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_
 
 #endif
 
-#ifdef DEBUG_WHEEL
+// Why was this DEBUG_WHEEL
+#if CHECK_VERSION(2026)
 WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const
 {
     // The generalized D matrix natively handles the w -> tangential velocity

src/software/physics/euclidean_to_wheel.h

@@ -7,8 +7,6 @@
 #include "software/geom/angular_velocity.h"
 #include "software/geom/vector.h"
 
-#define DEBUG_WHEEL
-
 /**
  * Vector representation of 2D Euclidean space.
  *

src/software/physics/euclidean_to_wheel_test.cpp

@@ -40,74 +40,104 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_zero)
 // velocity, and vise-versa.
 TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_x)
 {
-    // Test +x/right
+    // Test +x/forward
     target_euclidean_velocity = {1, 0, 0};
     calculated_wheel_speeds =
         euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
 
-    // Front wheels must be - velocity, back wheels must be + velocity.
+    // Right wheels must be - velocity, left wheels must be + velocity.
     EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
     EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
+
+
+    // Right wheels must have same velocity magnitude as left wheels, but opposite sign.
+    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX],
+                     -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]);
+    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX],
+                     -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]);
 }
 
 TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_x)
 {
-    // Test -x/left
+    // Test -x/backward
     target_euclidean_velocity = {-1, 0, 0};
     calculated_wheel_speeds =
         euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
 
-    // Front wheels must be + velocity, back wheels must be - velocity.
+    // Right wheels must be + velocity, left wheels must be - velocity.
     EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
     EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
+
+    // Right wheels must have same velocity magnitude as left wheels, but opposite sign.
+    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX],
+                     -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]);
+    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX],
+                     -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]);
 }
 
 TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_y)
 {
-    // Test +y/forwards
+    // Test +y/left
     target_euclidean_velocity = {0, 1, 0};
     calculated_wheel_speeds =
         euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
 
-    // Right wheels must be + velocity, Left wheels must be - velocity.
-    EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
+    // Back wheels must be + velocity, front wheels must be - velocity.
+    EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
     EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
     EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
-
-    // Right wheels must have same velocity magnitude as left wheels, but opposite sign.
-    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX],
-                     -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]);
-    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX],
-                     -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]);
 }
 
 TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y)
 {
-    // Test -y/backwards
+    // Test -y/right
     target_euclidean_velocity = {0, -1, 0};
     calculated_wheel_speeds =
         euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
 
+    // Back wheels must be - velocity, front wheels must be + velocity.
+    EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
+}
+
+TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign)
+{
+    // Test w / counter-clockwise
+    target_euclidean_velocity = {0, 0, 1};
+    calculated_wheel_speeds =
+        euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
+
     // Right wheels must be + velocity, Left wheels must be - velocity.
     EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
-    EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
     EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
+}
 
-    // Right wheels must have same velocity magnitude as left wheels, but opposite sign.
-    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX],
-                     -calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]);
-    EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX],
-                     -calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]);
+TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign)
+{
+    // Test w / clockwise
+    target_euclidean_velocity = {0, 0, -1};
+    calculated_wheel_speeds =
+        euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity);
+
+    // Right wheels must be + velocity, Left wheels must be - velocity.
+    EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
 }
 
-#ifdef DEBUG_WHEEL
-TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w)
+// #ifdef DEBUG_WHEEL
+#if CHECK_VERSION(2026)
+TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude)
 {
     // Test +w/counter-clockwise
     target_euclidean_velocity = {0, 0, 1};
@@ -116,20 +146,23 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w)
 
     // Because the wheels are offset, their speed at 1 rad/s equals their
     // exact physical lever arm (in meters), NOT the nominal robot_radius.
-    // Based on the physical offsets, the lever arm evaluates to 0.0746 meters.
-    double expected_lever_arm = 0.0746;
 
-    EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX],
+    // i have no idea what it's yapping about above but i ignored it lol
+
+    // Based on the physical offsets, the lever arm evaluates to 0.0749 meters.
+    double expected_lever_arm = 0.0749;
+
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]),
                 expected_lever_arm, 0.001);
-    EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm,
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm,
                 0.001);
-    EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm,
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm,
                 0.001);
-    EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm,
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm,
                 0.001);
 }
 
-TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w)
+TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude)
 {
     // Test -w/clockwise
     target_euclidean_velocity = {0, 0, -1};
@@ -138,20 +171,20 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w)
 
     // Because the wheels are offset, their speed at -1 rad/s equals their
     // exact physical lever arm (in meters) multiplied by the negative velocity.
-    double expected_lever_arm = -0.0746;
+    double expected_lever_arm = 0.0749;
 
-    EXPECT_NEAR(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX],
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]),
                 expected_lever_arm, 0.001);
-    EXPECT_NEAR(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm,
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm,
                 0.001);
-    EXPECT_NEAR(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], expected_lever_arm,
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), expected_lever_arm,
                 0.001);
-    EXPECT_NEAR(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], expected_lever_arm,
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), expected_lever_arm,
                 0.001);
 }
 #else
 
-TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w)
+TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude)
 {
     // Test +w/counter-clockwise
     target_euclidean_velocity = {0, 0, 1};
@@ -169,7 +202,7 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w)
 }
 
 
-TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w)
+TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_magnitude)
 {
     // Test -w/clockwise
     target_euclidean_velocity = {0, 0, -1};