more euclid change

Author: Andrewyx

src/software/physics/euclidean_to_wheel.cpp

@@ -37,10 +37,10 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_
 
     // 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;
+         cos_p, -sin_p, rot_f,
+        -cos_p, -sin_p, rot_f,
+        -cos_t,  sin_t, rot_b,
+         cos_t,  sin_t, rot_b;
     // clang-format on
 
     // Calculate Pseudo-inverse dynamically

src/software/physics/euclidean_to_wheel_test.cpp

@@ -40,70 +40,70 @@ 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/forwards
     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.
-    EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
+    // Right wheels must be + velocity, Left wheels must be - velocity.
+    EXPECT_GT(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[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
     EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0);
 }
 
 TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_x)
 {
-    // Test -x/left
+    // Test -x/backwards
     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.
-    EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
+    // 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_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0);
+    EXPECT_GT(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_y)
 {
-    // Test +y/forwards
+    // Test +y/right
     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);
+    // Front wheels must be - velocity, back 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]);
+                     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]);
+                     calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]);
 }
 
 TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y)
 {
-    // Test -y/backwards
+    // 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_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0);
+    // Front wheels must be + velocity, back 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_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]);
+                     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]);
+                     calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]);
 }
 
 #ifdef DEBUG_WHEEL