Improved robot localization and trajectory following by williamckha · Pull Request #3716 · UBC-Thunderbots/Software

williamckha · 2026-05-18T05:24:12Z

Description

Warning

This PR will be broken down into several smaller work items. Do not merge.

Testing Done

Resolved Issues

Foundations for #3703, #3704, #3705, #3706

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

Andrewyx · 2026-05-20T02:34:02Z

@@ -0,0 +1,149 @@
+#include "imu.h"


Separate into IMU PR per #3703 for thorough implementation

Andrewyx · 2026-05-20T02:36:02Z

+bool PrimitiveExecutor::isLinearTrajectoryNew(
+    const std::optional<TrajectoryPath>& new_trajectory) const
+{
+    if (new_trajectory.has_value() != trajectory_path_.has_value())
+    {
+        // Either we don't have a trajectory right now, and this one does, or we have one
+        // and this trajectory doesn't.
+        return true;
+    }
+
+    if (!new_trajectory.has_value())
+    {
+        return false;
+    }
+
+    return distance(new_trajectory->getDestination(),
+                    trajectory_path_->getDestination()) >
+           LINEAR_DESTINATION_THRESHOLD_METERS;
+}
+
+bool PrimitiveExecutor::isAngularTrajectoryNew(
+    const BangBangTrajectory1DAngular& new_trajectory) const
+{
+    if (!angular_trajectory_.has_value())
+    {
+        return true;
+    }
+
+    return new_trajectory.getDestination()
+               .minDiff(angular_trajectory_->getDestination())
+               .toDegrees() > ANGULAR_DESTINATION_THRESHOLD_DEGREES;
+}


Create a TODO PR to extract this into a delegate class. SRP code smell for prim executor

Andrewyx · 2026-05-20T02:39:21Z

+    /**
+     * Check if the given trajectory is "new". That is, if its destination differs
+     * meaningfully from our current trajectory.
+     *
+     * @param new_trajectory The new trajectory requested by the AI.
+     * @return True if the new trajectory requested is meaningfully different from the
+     * current trajectory. That is, if the destinations are new.
+     */
+    bool isLinearTrajectoryNew(const std::optional<TrajectoryPath>& new_trajectory) const;
+
+    /**
+     * Check if the given trajectory is "new". That is, if its destination differs
+     * meaningfully from our current trajectory.
+     *
+     * @param new_trajectory The new trajectory requested by the AI.
+     * @return True if the new trajectory requested is meaningfully different from the
+     * current trajectory. That is, if the destinations are new.
+     */
+    bool isAngularTrajectoryNew(const BangBangTrajectory1DAngular& new_trajectory) const;


TODO: Move to trajectory planner as static methods?

Andrewyx · 2026-05-20T02:51:26Z

+    static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13;
+
+    static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS   = 0.5;
+    static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25;


TODO: Keep Primitive Executor lightweight, move the tuning logic to lower level. Reference with interface e.g.
VelocityController

Template type with linear or angular variants? VelocityController<BangBangTrajectory1DAngular>?

Andrewyx · 2026-05-20T03:06:14Z

+    /**
+     * Update the robot's position and orientation from data reported by vision.
+     *
+     * @param position Vision reading of the robot's position in world space
+     * @param orientation Vision reading of the robot's orientation in world space
+     * @param age_seconds Age in seconds of the vision snapshot (time since it was taken)
+     */
+    void updateVision(const Point& position, const Angle& orientation,
+                      double age_seconds);
+
+    /**
+     * Update the robot's velocity from data reported by motor sensors
+     * (i.e. encoders or Hall sensors).
+     *
+     * @param velocity Motor sensor reading of the robot's velocity in local space
+     * @param angular_velocity Motor sensor reading of the robot's angular velocity
+     */
+    void updateMotorSensors(const Vector& velocity,
+                            const AngularVelocity& angular_velocity);
+
+    /**
+     * Update the angular velocity from IMU.
+     *
+     * @param angular_velocity angular velocity of the robot
+     */
+    void updateImu(const AngularVelocity& angular_velocity);


Code smell. Should be 1 unified update method. Consider using either method overloading or template

I disagree here, we need different implementation for different sensors no? And sensors can provide the same types of measurements so method overloading does not work here

To be clear, our kalman filter differentiates between measurements from different sensors

I agree with William that the methods should stay separate. However, the implementations do look very similar, so there is probably an abstraction via a helper function that could be created

Andrewyx · 2026-05-20T03:09:10Z

    std::unique_ptr<MotorService> motor_service_;
    std::unique_ptr<NetworkService> network_service_;
    std::unique_ptr<PowerService> power_service_;
+    std::unique_ptr<ImuService> imu_service_;


TODO: Interface

Andrewyx · 2026-05-20T03:10:21Z

+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);
+
+    // 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;
+
+    // 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));
+
+    // 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;
+    // clang-format on
+
+    // Calculate Pseudo-inverse dynamically
+    wheel_to_euclidean_velocity_D_inverse_ =
+        (euclidean_to_wheel_velocity_D_.transpose() * euclidean_to_wheel_velocity_D_)
+            .inverse() *
+        euclidean_to_wheel_velocity_D_.transpose();
+}
+
+#else


TODO: #3722

Andrewyx · 2026-05-20T03:11:09Z

@@ -0,0 +1,151 @@
+#pragma once


TODO: Merge with localizer or IMU impl, or on its own

nycrat · 2026-05-20T19:02:26Z

+std::map<RobotId, std::tuple<Point, Angle, Vector, AngularVelocity>>
+ErForceSimulator::getRobotIdToStateMap(


Should create struct that contains the state instead of using a tuple. Could be reused in many places

Or just use the RobotState class that already exists

nycrat · 2026-05-20T22:35:36Z

Separated implementation of RobotLocalizer to other PR #3725

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

…nderbots into tbots/motion

williamckha requested review from Andrewyx and sauravbanna as code owners May 18, 2026 05:24

This was linked to issues May 18, 2026

SPIKE IMU Integration #3703

Open

Non-Ephemeral Resilient Trajectories #3704

Open

Detect Stalled Trajectories #3705

Open

New Trajectory Detection #3706

Open

williamckha force-pushed the tbots/motion branch from 17408d5 to efafecc Compare May 18, 2026 18:26

Andrewyx reviewed May 20, 2026

View reviewed changes

nycrat reviewed May 20, 2026

View reviewed changes

nycrat mentioned this pull request May 20, 2026

Implement Robot Localizer #3725

Open

3 tasks

Andrewyx mentioned this pull request May 20, 2026

Implement VelocityController and PIDController #3726

Open

2 tasks

nycrat mentioned this pull request May 21, 2026

Add kalman filter implementation #3728

Merged

4 tasks

Andrewyx mentioned this pull request May 21, 2026

SPIKE IMU Integration #3703

Open

4 tasks

StarrryNight and others added 9 commits May 21, 2026 20:01

intiilaize imu.h

baa4c90

finish imu.h

7aaec7e

init imu.cpp

6d68e74

finish imu.cpp

f466385

update imu cpp and add thunderloop test

4199e76

Refactor motor service and implement STSPIN motor controller

48ccb08

Run lint and format

448900b

Improved robot localization and trajectory following

69cbf9e

Revert temporary changes

1fbc65a

williamckha and others added 4 commits May 22, 2026 18:36

Delete empty robot_constants.cpp

ab9e118

Changes to primitive executor

d4b2a76

Improved Euclidean to Wheel Readability / Logic? + Fixed Tests

fd921ee

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

f86b25d

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

williamckha force-pushed the tbots/motion branch from 3dc448b to f86b25d Compare May 23, 2026 01:37

StarrryNight added 4 commits May 23, 2026 11:38

initialize velocity related stuff

a874142

remove leftover

fad0376

fix thunderloop

4d9e8ca

cleanup

cfa79b9

StarrryNight mentioned this pull request May 23, 2026

IMU Integration #3735

Closed

4 tasks

StarrryNight added 8 commits May 23, 2026 13:15

fix bug

3442db3

fix build

82aa4af

add angular acceleration method

8add699

fix BUILD

b638f35

fix bugs

e7d12d3

merge

399cee6

unify poll function and introduce struct

6df8837

finish transform linear acceleration and fix bugs

b8a995e

This was referenced May 23, 2026

Implement PID/Position/Orientation controllers #3730

Open

Tune PidController constants #3737

Open

cleanup

a8397c6

StarrryNight mentioned this pull request May 23, 2026

IMU Integration #3739

Open

4 tasks

StarrryNight and others added 8 commits May 23, 2026 16:00

fix typo

437a9a8

resolve comments

c17b251

fix linear acceleration scope bug

6eeb1e8

fix thunderloop log bug

be15a2a

[pre-commit.ci lite] apply automatic fixes

038c1f2

Use elapsed time between stepPrimitive calls instead of fixed time step

3232093

Merge branch 'imu_integration' of https://github.com/StarrryNight/Thu…

87d7325

…nderbots into tbots/motion

Fix incorrect control model in robot localizer

aff8848

		std::map<RobotId, std::tuple<Point, Angle, Vector, AngularVelocity>>
		ErForceSimulator::getRobotIdToStateMap(

Conversation

williamckha commented May 18, 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!

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!

Andrewyx May 20, 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!

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!

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!

Choose a reason for hiding this comment

Uh oh!

nycrat May 20, 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!

Choose a reason for hiding this comment

Uh oh!

nycrat commented May 20, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants

williamckha commented May 18, 2026 •

edited by Andrewyx

Loading

Andrewyx May 20, 2026 •

edited

Loading

nycrat May 20, 2026 •

edited

Loading