diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 93ceb390cd..00a77c94da 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -10,30 +10,34 @@ 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; @@ -41,10 +45,34 @@ struct RobotConstants // 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() diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 1b9cde8dac..63c24d9666 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -11,26 +11,38 @@ 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 @@ -38,47 +50,25 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ // 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( diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index b546c9f01f..0890f6cde9 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -15,6 +15,9 @@ class EuclideanToWheelTest : public ::testing::Test robot_constants::RobotConstants robot_constants = robot_constants::createRobotConstants(); double robot_radius = robot_constants::createRobotConstants().robot_radius_m; + double expected_lever_arm = + robot_constants::createRobotConstants().expected_lever_arm; + const float EQUALITY_COMPARISON = 0.001f; WheelSpace_t target_wheel_velocity{}; WheelSpace_t current_wheel_velocity{}; @@ -40,109 +43,142 @@ 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); } -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}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Formula for the length of a segment: length = radius * angle - // Since angle = 1rad, the length of the segment is equal to the radius. - // Therefore, all wheel speeds must be equal to the robot radius. - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], - robot_radius); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], robot_radius); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], robot_radius); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], robot_radius); + // Because the wheels are offset, their speed at 1 rad/s equals their + // exact physical lever arm (in meters), NOT the nominal robot_radius. + + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); } -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}; calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Formula for the length of a segment: length = radius * angle - // Since angle = -1rad, the length of the segment is equal to the -radius. - // Therefore, all wheel speeds (=length of segment/sec) must be equal to the robot - // radius. - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], - -robot_radius); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], - -robot_radius); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], -robot_radius); - EXPECT_DOUBLE_EQ(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], - -robot_radius); + // Because the wheels are offset, their speed at -1 rad/s equals their + // exact physical lever arm (in meters) multiplied by the negative velocity. + + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); + EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); + EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]), + expected_lever_arm, EQUALITY_COMPARISON); } + TEST_F(EuclideanToWheelTest, test_conversion_is_linear) { target_euclidean_velocity = {3, 1, 5};