Euclidean Debug by sunghyuneun · Pull Request #3731 · UBC-Thunderbots/Software

sunghyuneun · 2026-05-22T03:11:22Z

Description

Rewrote the inverse kinematics logic for increased readability and correctness.
Revised wrong tests to be correct.

Testing Done

Also edited euclidean_to_wheel_test.cpp, and passed all tests in it.

Resolved Issues

Resolves #3722

Length Justification and Key Files to Review

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
Remove all commented out code
Remove extra print statements: for example, those just used for testing
Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue

…hyuneun/tb-software into sunghyuneun/euclidean_debug

Moved changes to PR #3731 This reverts commit 6aec7b3.

Formatting accidentally deleted a requirements_lock This reverts commit 7f7fb0f.

Andrewyx

Nice work! Left some comments

Andrewyx · 2026-05-22T21:24:58Z

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


We should store these constants in robot_constants.h alongside and also update the diagram for the wheel orientations in the meantime

Andrewyx · 2026-05-22T21:25:29Z

+        euclidean_to_wheel_velocity_D_.transpose();
+}
+
+#else


Either delete the legacy code or add a TODO & issue for removing this old code in the future.

Andrewyx · 2026-05-22T21:34:22Z

+         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


We know all of these values statically, is there some way to compute everything before runtime in this case? Can this entire constructor be constexpr?

Andrewyx · 2026-05-22T21:45:13Z

+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX]),
+                expected_lever_arm, 0.001);
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX]),
+                expected_lever_arm, 0.001);
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX]),
+                expected_lever_arm, 0.001);
+    EXPECT_NEAR(std::abs(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX]),
+                expected_lever_arm, 0.001);


Could we use https://en.cppreference.com/cpp/types/numeric_limits/epsilon?

Andrewyx · 2026-05-22T21:46:11Z

+
+    // 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.0749;


Magic number

Andrewyx · 2026-05-22T21:48:35Z

+    auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
+    auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;


No need for auto here, and also statically known values

Moved changes to PR #3731 This reverts commit 6aec7b3.

Andrewyx

Almost there, left some comments!

Andrewyx · 2026-05-23T19:59:31Z

+    // X position of centre of front wheel
+    float front_wheel_x_mag;
+
+    // Y position of centre of front wheel
+    float front_wheel_y_mag;
+
+    // X position of centre of rear wheel
+    float back_wheel_x_mag;
+
+    // Y position of centre of rear wheel
+    float back_wheel_y_mag;
+


what is mag? Note that we should ideally keep units in the name too e.g. front_wheel_x_meters -> // X position of centre of front wheel relative to the center of the robot.

nit: we should replace magnitude with pos, because I think position is more specific than just magnitude.

Andrewyx · 2026-05-23T20:00:25Z

-        .wheel_radius_meters = 0.03f};
+        .wheel_radius_meters = 0.03f,
+
+        .expected_lever_arm = 0.0749f};


How is this computed? Also can you label this somewhere in the ASCII diagram or something to better explain what this value points to?

StarrryNight

nice work. Left some comment on code quality and inversing matrices.

StarrryNight

lgtm

Andrewyx · 2026-05-26T18:22:01Z

+        .front_wheel_x_magnitude_meters = 0.03485f,
+        .front_wheel_y_magnitude_meters = 0.06632f,
+        .back_wheel_x_magnitude_meters  = 0.04985f,
+        .back_wheel_y_magnitude_meters  = 0.05592f,
+


nice placement

Andrewyx · 2026-05-26T18:26:17Z

+    // Computed by finding sqrt(x^2 + y^2) of front and rear wheels. Both equal
+    float expected_lever_arm;
 };


perhaps link some sort of graphic or wiki link here? I am still a bit confused as to what this is referring to wrt the geometry of the robot

Andrewyx · 2026-05-26T18:27:19Z

Nice work! This looks basically done, I left two very minor comments regarding docs, but otherwise, I think this is ready to go 🚀

…hyuneun/tb-software into sunghyuneun/euclidean_debug

Andrewyx

Fantastic work! 💯

GrayHoang

Looks good, good work on this

GrayHoang · 2026-05-28T23:45:29Z

+    //                 FRONT OF ROBOT
+    //         |             ▲ X-Axis
+    //         |   /  -------+---------  \                     eric
+    //         |A / .'       │         '. \  ◄─── Front wheel       
+    //         | /.'         │          .'.\                   grayson      
+    //         |//           │       .     \\                  samuel
+    //         ;             │    . Lever    ;     
+    //        |              │ .    Arm      |      
+    //  ◄─────┼──────────────┼               |      
+    //  Y-Axis|                              |      
+    //         ;                             ;      
+    //         |\\                         //       
+    //         | \'.                     .'/        
+    //         |B \ '.                 .' / ◄─── Back wheel        
+    //         |   \  '-.          _.-'  /          
+    //         |         ''------''
+
+    // clang-format on


this ASCII art is ass-key. Make better? :)

GrayHoang · 2026-05-28T23:46:43Z

    float robot_max_ang_acceleration_rad_per_s_2;

-    // The radius of the wheel, in meters
+    // The radius of the wheel, in metres


alr who's french

WHAT THE FUCK IS A MILE

Improved Euclidean to Wheel Readability / Logic? + Fixed Tests

0ae208a

sunghyuneun requested review from Andrewyx, sauravbanna and williamckha as code owners May 22, 2026 03:11

pre-commit-ci-lite Bot and others added 3 commits May 22, 2026 03:17

[pre-commit.ci lite] apply automatic fixes

3cc10dd

Fixed some formatting

65eaac3

Merge branch 'sunghyuneun/euclidean_debug' of https://github.com/sung…

f53aee3

…hyuneun/tb-software into sunghyuneun/euclidean_debug

sunghyuneun added a commit that referenced this pull request May 22, 2026

Revert "Improved Euclidean to Wheel Readability / Logic? + Fixed Tests"

3dc448b

Moved changes to PR #3731 This reverts commit 6aec7b3.

pre-commit-ci-lite Bot and others added 2 commits May 22, 2026 03:47

[pre-commit.ci lite] apply automatic fixes

7f7fb0f

Revert "[pre-commit.ci lite] apply automatic fixes"

167f589

Formatting accidentally deleted a requirements_lock This reverts commit 7f7fb0f.

sunghyuneun self-assigned this May 22, 2026

pre-commit-ci-lite Bot and others added 2 commits May 22, 2026 19:23

[pre-commit.ci lite] apply automatic fixes

835c2ac

Delete silly comment

e541f5d

Andrewyx reviewed May 22, 2026

View reviewed changes

sunghyuneun and others added 4 commits May 22, 2026 15:47

Removed magic numbers and removed old code in preprocessor directives

f4c6cd7

Another magic number is now labelled

61c9e5d

Changed auto type to static

86a81d3

[pre-commit.ci lite] apply automatic fixes

7262200

williamckha pushed a commit that referenced this pull request May 23, 2026

Revert "Improved Euclidean to Wheel Readability / Logic? + Fixed Tests"

f86b25d

Moved changes to PR #3731 This reverts commit 6aec7b3.

sunghyuneun requested a review from Andrewyx May 23, 2026 04:19

Andrewyx reviewed May 23, 2026

View reviewed changes

sunghyuneun and others added 4 commits May 23, 2026 15:03

updated ascii

5b55d1c

clang formatting

2fc875c

clang formatting part 2

1f2f5ac

[pre-commit.ci lite] apply automatic fixes

501ab89

sunghyuneun requested a review from Andrewyx May 23, 2026 23:16

StarrryNight reviewed May 24, 2026

View reviewed changes

Comment thread src/software/physics/euclidean_to_wheel.cpp Outdated

Comment thread src/software/physics/euclidean_to_wheel.cpp Outdated

Comment thread src/software/physics/euclidean_to_wheel.cpp Outdated

sunghyuneun and others added 2 commits May 23, 2026 19:17

Const Declarations, Inverse Rewritten

78245e1

[pre-commit.ci lite] apply automatic fixes

6455b48

StarrryNight previously approved these changes May 24, 2026

View reviewed changes

Andrewyx reviewed May 26, 2026

View reviewed changes

sunghyuneun added 2 commits May 26, 2026 13:32

Finishing touches

68b301a

Merge branch 'sunghyuneun/euclidean_debug' of https://github.com/sung…

11cb3c1

…hyuneun/tb-software into sunghyuneun/euclidean_debug

sunghyuneun dismissed StarrryNight’s stale review via 11cb3c1 May 26, 2026 21:19

[pre-commit.ci lite] apply automatic fixes

32d0eec

sunghyuneun requested a review from Andrewyx May 26, 2026 21:27

Andrewyx approved these changes May 27, 2026

View reviewed changes

GrayHoang approved these changes May 28, 2026

View reviewed changes

sunghyuneun merged commit b84795f into UBC-Thunderbots:master May 29, 2026
10 of 11 checks passed

		auto p = robot_constants_.front_wheel_angle_deg * M_PI / 180.;
		auto t = robot_constants_.back_wheel_angle_deg * M_PI / 180.;

Conversation

sunghyuneun commented May 22, 2026 • edited by Andrewyx Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Testing Done

Resolved Issues

Length Justification and Key Files to Review

Review Checklist

Uh oh!

Andrewyx left a comment

Choose a reason for hiding this comment

Uh oh!

Andrewyx May 22, 2026 • edited by sunghyuneun Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andrewyx May 22, 2026 • edited by sunghyuneun Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andrewyx May 22, 2026 • edited by sunghyuneun Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andrewyx left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

StarrryNight left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

StarrryNight left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Andrewyx commented May 26, 2026

Uh oh!

Andrewyx left a comment

Choose a reason for hiding this comment

Uh oh!

GrayHoang left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

sunghyuneun May 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

sunghyuneun commented May 22, 2026 •

edited by Andrewyx

Loading

Andrewyx May 22, 2026 •

edited by sunghyuneun

Loading

Andrewyx May 22, 2026 •

edited by sunghyuneun

Loading

Andrewyx May 22, 2026 •

edited by sunghyuneun

Loading

sunghyuneun May 29, 2026 •

edited

Loading