Euclidean Debug

src/shared/robot_constants.h

@@ -10,41 +10,69 @@ struct RobotConstants
     // The radius of the robot [m]
     float robot_radius_m;
 
+    // clang-format off
+
     // The front_wheel_angle_deg and back_wheel_angle_deg are measured as absolute
     // angles from the robot's y-axis to each wheel axle.
     //
     // In the ASCII diagram below:
     //  - front_wheel_angle_deg = A
     //  - back_wheel_angle_deg  = B
-    //
     // The angles are assumed to be symmetric for the left and right sides of the robot.
     //
-    //                        y
-    //                        ▲
-    //                        |
-    //    Back wheel          │         Front wheel
-    //        └────────►  , - │ - ,  ◄───────┘
-    //                , '\    │    /' ,
-    //              ,     \ B │ A /    │
-    //             ,       \  │  /     │
-    //            ,         \ │ /      │
-    //            ,           └────────┼───────► x   Front of robot
-    //            ,                    │
-    //             ,                   │
-    //              ,                  │
-    //                ,              .'
-    //                  ' - , _  , '
+    //                 FRONT OF ROBOT
+    //         |             ▲ X-Axis
+    //         |   /  -------+---------  \                     eric
+    //         |A / .'       │         '. \  ◄─── Front wheel       
+    //         | /.'         │          .'.\                   grayson      
+    //         |//           │       .     \\                  samuel
+    //         ;             │    . Lever    ;     
+    //        |              │ .    Arm      |      
+    //  ◄─────┼──────────────┼               |      
+    //  Y-Axis|                              |      
+    //         ;                             ;      
+    //         |\\                         //       
+    //         | \'.                     .'/        
+    //         |B \ '.                 .' / ◄─── Back wheel        
+    //         |   \  '-.          _.-'  /          
+    //         |         ''------''
+
+    // clang-format on
 
     // The angle between y-axis of the robot and the front wheel axles [degrees]
     float front_wheel_angle_deg;
 
     // The angle between y-axis of the robot and the rear wheel axles [degrees]
     float back_wheel_angle_deg;
 
-    // The total width of the entire flat face on the front of the robot [meters]
+    // X position of centre of front right wheel
+    float fr_x_pos_meters;
+
+    // Y position of centre of front right wheel
+    float fr_y_pos_meters;
+
+    // X position of centre of front left wheel
+    float fl_x_pos_meters;
+
+    // Y position of centre of front left wheel
+    float fl_y_pos_meters;
+
+    // X position of centre of back left wheel
+    float bl_x_pos_meters;
+
+    // Y position of centre of back left wheel
+    float bl_y_pos_meters;
+
+    // X position of centre of back right wheel
+    float br_x_pos_meters;
+
+    // Y position of centre of back right wheel
+    float br_y_pos_meters;
+
+    // The total width of the entire flat face on the front of the robot [metres]
     float front_of_robot_width_meters;
 
-    // The distance from one end of the dribbler to the other [meters]
+    // The distance from one end of the dribbler to the other [metres]
     float dribbler_width_meters;
 
     // Indefinite dribbler mode sets a speed that can be maintained indefinitely [rpm]
@@ -72,8 +100,13 @@ struct RobotConstants
     // The maximum angular acceleration achievable by our robots [rad/s^2]
     float robot_max_ang_acceleration_rad_per_s_2;
 
-    // The radius of the wheel, in meters
+    // The radius of the wheel, in metres
     float wheel_radius_meters;
+
+    // Distance [metres] from centre of robot to centre of wheel.
+    // Found by sqrt(x^2 + y^2) of a wheel.
+    // Front wheel arm = Rear wheel arm. See ASCII image above.
+    float expected_lever_arm;
 };
 
 /**
@@ -89,6 +122,15 @@ constexpr RobotConstants createRobotConstants()
         .front_wheel_angle_deg = 32.0f,
         .back_wheel_angle_deg  = 44.0f,
 
+        .fr_x_pos_meters = 0.03485f,
+        .fr_y_pos_meters = -0.06632f,
+        .fl_x_pos_meters = 0.03485f,
+        .fl_y_pos_meters = 0.06632f,
+        .bl_x_pos_meters = -0.04985f,
+        .bl_y_pos_meters = 0.05592f,
+        .br_x_pos_meters = -0.04985f,
+        .br_y_pos_meters = -0.05592f,
+
         .front_of_robot_width_meters = 0.11f,
         .dribbler_width_meters       = 0.07825f,
 
@@ -108,7 +150,9 @@ constexpr RobotConstants createRobotConstants()
         .robot_max_ang_speed_rad_per_s          = 10.0f,
         .robot_max_ang_acceleration_rad_per_s_2 = 30.0f,
 
-        .wheel_radius_meters = 0.03f};
+        .wheel_radius_meters = 0.03f,
+
+        .expected_lever_arm = 0.0749f};
 }
 #elif CHECK_VERSION(2021)
 constexpr RobotConstants createRobotConstants()

src/software/physics/euclidean_to_wheel.cpp

@@ -11,74 +11,64 @@
 EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_constants)
     : robot_constants_(robot_constants)
 {
-    // Phi, the angle between the hemisphere line of the robot and the front wheel axles
-    // [rads]
-    auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
-
-    // Theta, the angle between the hemisphere line of the robot and the rear wheel axles
-    // [rads]
-    auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;
-
-    // Caching repeated calculations
-    auto cos_p = std::cos(p);
-    auto cos_t = std::cos(t);
-    auto sin_p = std::sin(p);
-    auto sin_t = std::sin(t);
+    // Angles [rads]
+    const double p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
+    const double t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;
+
+    // Beta is angle of wheel rolling direction relative to X-axis
+    // LOOK AT (software)/src/shared/robot_constants.h to see X-axis
+    const double b_fr = -(M_PI - p);
+    const double b_fl = -p;
+    const double b_bl = t;
+    const double b_br = (M_PI - t);
+
+    // Mapped to the robot frame: +X = Forward, +Y = Left
+    const double fr_x = robot_constants_.fr_x_pos_meters;
+    const double fr_y = robot_constants_.fr_y_pos_meters;
+    const double fl_x = robot_constants_.fl_x_pos_meters;
+    const double fl_y = robot_constants_.fl_y_pos_meters;
+    const double bl_x = robot_constants_.bl_x_pos_meters;
+    const double bl_y = robot_constants_.bl_y_pos_meters;
+    const double br_x = robot_constants_.br_x_pos_meters;
+    const double br_y = robot_constants_.br_y_pos_meters;
+
+
+    // 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, 1,
-        -sin_p, -cos_p, 1,
-         sin_t, -cos_t, 1,
-         sin_t,  cos_t, 1;
+         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
 
     // Inverse of euclidean to wheel matrix (D) for converting wheel velocity to euclidean
     // velocity.
     // NOTE: The pseudoinverse given in the paper
     // (http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 17) is incorrect. The inverse
     // was calculated using Wolfram Alpha: https://bit.ly/3A08BJX
-    auto i = 1 / (2 * sin_p + 2 * sin_t);
 
-    auto j_denom = 2 * cos_p * cos_p + 2 * cos_t * cos_t;
-    auto j1      = cos_p / j_denom;
-    auto j2      = cos_t / j_denom;
+    // Calculate Pseudo-inverse dynamically
 
-    auto k_denom = 2 * sin_p + 2 * sin_t;
-    auto k1      = sin_t / k_denom;
-    auto k2      = sin_p / k_denom;
-
-    // clang-format off
-    wheel_to_euclidean_velocity_D_inverse_ <<
-        -i,  -i,   i,  i,
-        j1, -j1, -j2, j2,
-        k1,  k1,  k2, k2;
-    // clang-format on
+    wheel_to_euclidean_velocity_D_inverse_ =
+        euclidean_to_wheel_velocity_D_.completeOrthogonalDecomposition().pseudoInverse();
 }
 
 WheelSpace_t EuclideanToWheel::getWheelVelocity(EuclideanSpace_t euclidean_velocity) const
 {
-    // need to multiply the angular velocity by the robot radius to
-    // calculate the wheel velocity (robot tangential velocity)
-    // ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8
-    euclidean_velocity[2] = euclidean_velocity[2] * robot_constants_.robot_radius_m;
-
+    // The generalized D matrix natively handles the w -> tangential velocity
+    // conversion because its third column already represents the lever arm in meters.
     return euclidean_to_wheel_velocity_D_ * euclidean_velocity;
 }
 
 EuclideanSpace_t EuclideanToWheel::getEuclideanVelocity(
     const WheelSpace_t& wheel_velocity) const
 {
-    EuclideanSpace_t euclidean_velocity =
-        wheel_to_euclidean_velocity_D_inverse_ * wheel_velocity;
-
-    // The above multiplication will calculate the tangential robot
-    // velocity. This can be divided by the robot radius to calculate
-    // the angular velocity
-    // ref: http://robocup.mi.fu-berlin.de/buch/omnidrive.pdf pg 8
-    euclidean_velocity[2] = euclidean_velocity[2] / robot_constants_.robot_radius_m;
-
-    return euclidean_velocity;
+    // The D inverse matrix natively outputs w in rad/s.
+    return wheel_to_euclidean_velocity_D_inverse_ * wheel_velocity;
 }
 
 WheelSpace_t EuclideanToWheel::rampWheelVelocity(