From 5a3fb6d2f3270c4618f2834f465c082a9b35b55d Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 14:38:34 -0700 Subject: [PATCH 01/18] Remove unused member functions and variables --- src/software/embedded/primitive_executor.cpp | 19 ++----------------- src/software/embedded/primitive_executor.h | 19 +------------------ src/software/embedded/thunderloop.cpp | 4 +--- .../simulation/er_force_simulator.cpp | 6 ++---- 4 files changed, 6 insertions(+), 42 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..86dd75f7d8 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,13 +11,8 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, const RobotId robot_id) - : current_primitive_(), - friendly_team_colour_(friendly_team_colour), - robot_constants_(robot_constants), - time_step_(time_step), - robot_id_(robot_id) + const Duration time_step, const robot_constants::RobotConstants& robot_constants) + : current_primitive_(), robot_constants_(robot_constants), time_step_(time_step) { } @@ -38,11 +33,6 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m } } -void PrimitiveExecutor::setStopPrimitive() -{ - current_primitive_ = *createStopPrimitiveProto(); -} - void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, const AngularVelocity& angular_velocity) { @@ -145,8 +135,3 @@ std::unique_ptr PrimitiveExecutor::stepPrimi } return std::make_unique(); } - -void PrimitiveExecutor::setRobotId(const RobotId robot_id) -{ - robot_id_ = robot_id; -} diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 145874f3c2..52d9de711e 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -17,13 +17,9 @@ class PrimitiveExecutor * @param time_step Time step which this primitive executor operates in * @param robot_constants The robot constants for the robot which uses this primitive * executor - * @param friendly_team_colour The colour of the friendly team - * @param robot_id The id of the robot which uses this primitive executor */ explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, - const RobotId robot_id); + const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive @@ -31,11 +27,6 @@ class PrimitiveExecutor */ void updatePrimitive(const TbotsProto::Primitive& primitive_msg); - /** - * Set the current primitive to the stop primitive - */ - void setStopPrimitive(); - /** * Update primitive executor with the current velocity of the robot * @@ -45,12 +36,6 @@ class PrimitiveExecutor void updateVelocity(const Vector& local_velocity, const AngularVelocity& angular_velocity); - /** - * Set the robot id - * @param robot_id The id of the robot which uses this primitive executor - */ - void setRobotId(RobotId robot_id); - /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities @@ -81,7 +66,6 @@ class PrimitiveExecutor Vector velocity_; AngularVelocity angular_velocity_; Angle orientation_; - TeamColour friendly_team_colour_; robot_constants::RobotConstants robot_constants_; std::optional trajectory_path_; std::optional angular_trajectory_; @@ -89,7 +73,6 @@ class PrimitiveExecutor // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant // time step to be used, in Seconds Duration time_step_; - RobotId robot_id_; // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index afc30bdd03..11bfc8489f 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -71,7 +71,6 @@ extern "C" Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) - // TODO (#2495): Set the friendly team colour : toml_config_client_(std::make_unique(TOML_CONFIG_FILE_PATH)), motor_status_(std::nullopt), robot_constants_(robot_constants), @@ -84,8 +83,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_) + primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants) { waitForNetworkUp(); diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..c609bff582 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -281,15 +281,13 @@ void ErForceSimulator::setRobots( if (side == gameController::Team::BLUE) { auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::BLUE, id); + Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::YELLOW, id); + Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } From 868cf6b35c214a94249f18eaba4487f6ef636625 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 15:06:32 -0700 Subject: [PATCH 02/18] Fix call to setStopPrimitive this is removed even thought is technically used now cause future changes will get rid of it --- src/software/embedded/thunderloop.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 11bfc8489f..c2d9c08a70 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -4,6 +4,7 @@ #include #include "proto/message_translation/tbots_protobuf.h" +#include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -282,7 +283,7 @@ void Thunderloop::runLoop() if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) { - primitive_executor_.setStopPrimitive(); + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } direct_control_ = From 67b3833f3e3fc8069e43d4c6edc06dec7568134c Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 14:49:33 -0700 Subject: [PATCH 03/18] Pass in delta_time dynamically for prim exec --- src/software/embedded/primitive_executor.cpp | 8 ++++---- src/software/embedded/primitive_executor.h | 12 ++++-------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 86dd75f7d8..8961861eff 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,8 +11,8 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants) - : current_primitive_(), robot_constants_(robot_constants), time_step_(time_step) + const robot_constants::RobotConstants& robot_constants) + : current_primitive_(), robot_constants_(robot_constants) { } @@ -78,9 +78,9 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status) + TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time) { - time_since_trajectory_creation_ += time_step_; + time_since_trajectory_creation_ += delta_time; status.set_running_primitive(true); switch (current_primitive_.primitive_case()) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 52d9de711e..f0fb3d2253 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -14,12 +14,10 @@ class PrimitiveExecutor public: /** * Constructor - * @param time_step Time step which this primitive executor operates in * @param robot_constants The robot constants for the robot which uses this primitive * executor */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive @@ -39,13 +37,15 @@ class PrimitiveExecutor /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities + * * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive + * @param delta_time The elapsed time since the last primitive step * * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status); + TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time); private: /* @@ -70,10 +70,6 @@ class PrimitiveExecutor std::optional trajectory_path_; std::optional angular_trajectory_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; From dd126dbd05c29bd40162e1215af6a287cca970c5 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 15:03:43 -0700 Subject: [PATCH 04/18] Fix calls to prim exec constructor and stepPrimitive --- src/software/embedded/thunderloop.cpp | 4 +-- .../simulation/er_force_simulator.cpp | 31 ++++++++++--------- src/software/simulation/er_force_simulator.h | 16 +++++----- 3 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index c2d9c08a70..01445023b0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -84,7 +84,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants) + primitive_executor_(robot_constants) { waitForNetworkUp(); @@ -287,7 +287,7 @@ void Thunderloop::runLoop() } direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_); + *primitive_executor_.stepPrimitive(primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); } thunderloop_status_.set_primitive_executor_step_time_ms( diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index c609bff582..ba038290f0 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -20,10 +20,10 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping, - double primitive_executor_time_step) + Duration primitive_executor_time_step) : yellow_team_world_msg(std::make_unique()), blue_team_world_msg(std::make_unique()), - primitive_executor_time_step_s(primitive_executor_time_step), + primitive_executor_time_step(primitive_executor_time_step), frame_number(0), euclidean_to_four_wheel(robot_constants), robot_constants(robot_constants), @@ -280,14 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -389,15 +389,17 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( TbotsProto::PrimitiveExecutorStatus status; // Added for compilation if (ramping) { - auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); - direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, - primitive_executor_time_step_s); + auto direct_control_no_ramp = + primitive_executor->stepPrimitive(status, primitive_executor_time_step); + direct_control = getRampedVelocityPrimitive( + current_velocity_map.at(robot_id).first, + current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + primitive_executor_time_step); } else { - direct_control = primitive_executor->stepPrimitive(status); + direct_control = + primitive_executor->stepPrimitive(status, primitive_executor_time_step); } auto command = *getRobotCommandFromDirectControl( @@ -411,8 +413,7 @@ std::unique_ptr ErForceSimulator::getRampedVelocityPrimitive( const Vector current_local_velocity, const AngularVelocity current_local_angular_velocity, - TbotsProto::DirectControlPrimitive& target_velocity_primitive, - const double& time_to_ramp) + TbotsProto::DirectControlPrimitive& target_velocity_primitive, Duration time_to_ramp) { TbotsProto::MotorControl_DirectVelocityControl direct_velocity = target_velocity_primitive.motor_control().direct_velocity_control(); @@ -435,7 +436,7 @@ ErForceSimulator::getRampedVelocityPrimitive( euclidean_to_four_wheel.getWheelVelocity(current_euclidean_velocity); WheelSpace_t ramped_four_wheel = euclidean_to_four_wheel.rampWheelVelocity( - current_wheel_velocity, target_wheel_velocity, time_to_ramp); + current_wheel_velocity, target_wheel_velocity, time_to_ramp.toSeconds()); EuclideanSpace_t ramped_euclidean = euclidean_to_four_wheel.getEuclideanVelocity(ramped_four_wheel); diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..429b16d503 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -27,12 +27,12 @@ class ErForceSimulator * @param robot_constants The robot constants * @param realism_config realism configuration */ - explicit ErForceSimulator(const TbotsProto::FieldType& field_type, - const robot_constants::RobotConstants& robot_constants, - std::unique_ptr& realism_config, - const bool ramping = false, - double primitive_executor_time_step_s = - DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK); + explicit ErForceSimulator( + const TbotsProto::FieldType& field_type, + const robot_constants::RobotConstants& robot_constants, + std::unique_ptr& realism_config, const bool ramping = false, + Duration primitive_executor_time_step_s = + Duration::fromSeconds(DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK)); ErForceSimulator() = delete; ~ErForceSimulator() = default; @@ -202,7 +202,7 @@ class ErForceSimulator const Vector current_local_velocity, const AngularVelocity current_local_angular_velocity, TbotsProto::DirectControlPrimitive& target_velocity_primitive, - const double& time_to_ramp); + Duration time_to_ramp); // Map of Robot id to Primitive Executor std::unordered_map> @@ -212,7 +212,7 @@ class ErForceSimulator std::unique_ptr yellow_team_world_msg; std::unique_ptr blue_team_world_msg; - double primitive_executor_time_step_s; + Duration primitive_executor_time_step; unsigned int frame_number; // The current time. From c5342f33f401cda945c54fe3e59230c9cac8d80a Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 15:34:48 -0700 Subject: [PATCH 05/18] Add setters to RobotState class --- src/software/world/robot_state.cpp | 20 ++++++++++++++++++++ src/software/world/robot_state.h | 28 ++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/src/software/world/robot_state.cpp b/src/software/world/robot_state.cpp index 2e9f4c0b79..c1f1266622 100644 --- a/src/software/world/robot_state.cpp +++ b/src/software/world/robot_state.cpp @@ -48,6 +48,26 @@ bool RobotState::breakbeamTripped() const return breakbeam_tripped_; } +void RobotState::setPosition(const Point& position) +{ + position_ = position; +} + +void RobotState::setVelocity(const Vector& velocity) +{ + velocity_ = velocity; +} + +void RobotState::setOrientation(const Angle& orientation) +{ + orientation_ = orientation; +} + +void RobotState::setAngularVelocity(const AngularVelocity& angular_velocity) +{ + angular_velocity_ = angular_velocity; +} + bool RobotState::operator==(const RobotState& other) const { return this->position() == other.position() && this->velocity() == other.velocity() && diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index 9f62084931..bf2e234a58 100644 --- a/src/software/world/robot_state.h +++ b/src/software/world/robot_state.h @@ -75,6 +75,34 @@ class RobotState */ bool breakbeamTripped() const; + /** + * Sets the position of the robot, with coordinates in metres + * + * @param position The new position of the robot + */ + void setPosition(const Point& position); + + /** + * Sets the velocity of the robot, in metres per second + * + * @param velocity The new velocity of the robot + */ + void setVelocity(const Vector& velocity); + + /** + * Sets the orientation of the robot + * + * @param orientation The new orientation of the robot + */ + void setOrientation(const Angle& orientation); + + /** + * Sets the angular velocity of the robot + * + * @param angular_velocity The new angular velocity of the robot + */ + void setAngularVelocity(const AngularVelocity& angular_velocity); + /** * Defines the equality operator for a RobotState. RobotStates are equal if * all their members are equal From d449a951387929ed5a059a59ebc66366ec4c343e Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 15:39:12 -0700 Subject: [PATCH 06/18] Replace state variables with one RobotState --- src/software/embedded/primitive_executor.cpp | 26 +++++++++++--------- src/software/embedded/primitive_executor.h | 4 +-- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 8961861eff..c6ba6ff959 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -12,7 +12,9 @@ PrimitiveExecutor::PrimitiveExecutor( const robot_constants::RobotConstants& robot_constants) - : current_primitive_(), robot_constants_(robot_constants) + : state_(Point(), Vector(), Angle(), AngularVelocity()), + current_primitive_(), + robot_constants_(robot_constants) { } @@ -22,12 +24,13 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (current_primitive_.has_move()) { - trajectory_path_ = createTrajectoryPathFromParams( - current_primitive_.move().xy_traj_params(), velocity_, robot_constants_); + trajectory_path_ = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + state_.velocity(), robot_constants_); angular_trajectory_ = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - angular_velocity_, robot_constants_); + state_.angularVelocity(), robot_constants_); time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); } @@ -36,16 +39,17 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, const AngularVelocity& angular_velocity) { - Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_); - velocity_ = actual_global_velocity; - angular_velocity_ = angular_velocity; + Vector actual_global_velocity = + localToGlobalVelocity(local_velocity, state_.orientation()); + state_.setVelocity(actual_global_velocity); + state_.setAngularVelocity(angular_velocity); } Vector PrimitiveExecutor::getTargetLinearVelocity() { Vector local_velocity = globalToLocalVelocity( trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), - orientation_); + state_.orientation()); Point position = trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); double distance_to_destination = @@ -61,13 +65,13 @@ Vector PrimitiveExecutor::getTargetLinearVelocity() AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() { - orientation_ = - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds()); + state_.setOrientation( + angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds())); AngularVelocity angular_velocity = angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); Angle orientation_to_destination = - orientation_.minDiff(angular_trajectory_->getDestination()); + state_.orientation().minDiff(angular_trajectory_->getDestination()); if (orientation_to_destination.toDegrees() < 5) { angular_velocity *= orientation_to_destination.toDegrees() / 5; diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index f0fb3d2253..8a563b92f1 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -61,11 +61,9 @@ class PrimitiveExecutor */ AngularVelocity getTargetAngularVelocity(); + RobotState state_; TbotsProto::Primitive current_primitive_; Duration time_since_trajectory_creation_; - Vector velocity_; - AngularVelocity angular_velocity_; - Angle orientation_; robot_constants::RobotConstants robot_constants_; std::optional trajectory_path_; std::optional angular_trajectory_; From 3e929bafdb2dc1c25201f7cc7b02b2215a8648ce Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 16:02:54 -0700 Subject: [PATCH 07/18] Create default constructor for robot state --- src/software/embedded/primitive_executor.cpp | 4 +--- src/software/world/robot_state.h | 5 +++++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index c6ba6ff959..a64262f3dc 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -12,9 +12,7 @@ PrimitiveExecutor::PrimitiveExecutor( const robot_constants::RobotConstants& robot_constants) - : state_(Point(), Vector(), Angle(), AngularVelocity()), - current_primitive_(), - robot_constants_(robot_constants) + : current_primitive_(), robot_constants_(robot_constants) { } diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index bf2e234a58..fac609fa12 100644 --- a/src/software/world/robot_state.h +++ b/src/software/world/robot_state.h @@ -17,6 +17,11 @@ using RobotId = unsigned int; class RobotState { public: + /** + * Creates a robot state with default values + */ + RobotState() = default; + /** * Creates a new robot state with the given position, velocity, orientation, angular * velocity, and timestamp From 03da9c87c0757a56a20ecb77b02173ccd9993309 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 16:13:31 -0700 Subject: [PATCH 08/18] Replace use of std::pair with RobotState --- .../simulation/er_force_simulator.cpp | 77 ++++++++----------- src/software/simulation/er_force_simulator.h | 7 +- src/software/world/BUILD | 1 + src/software/world/robot_state.cpp | 7 ++ src/software/world/robot_state.h | 7 ++ 5 files changed, 52 insertions(+), 47 deletions(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index ba038290f0..ec7b69cf23 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -297,19 +297,19 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.yellow_robots(); + const auto robot_map = getRobotIdToRobotStateMap(sim_robots); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_map.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& robot_state = robot_map.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, robot_state); } } } @@ -318,20 +318,20 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.blue_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.blue_robots(); + const auto robot_map = getRobotIdToRobotStateMap(sim_robots); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_map.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& robot_state = robot_map.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, robot_state); } } } @@ -340,8 +340,7 @@ void ErForceSimulator::setRobotPrimitive( RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity) + const TbotsProto::World& world_msg, const RobotState& robot_state) { // Set to NEG_X because the world msg in this simulator is normalized // correctly @@ -351,7 +350,8 @@ void ErForceSimulator::setRobotPrimitive( { auto primitive_executor = robot_primitive_executor_iter->second; primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id)); - primitive_executor->updateVelocity(local_velocity, angular_velocity); + primitive_executor->updateVelocity(robot_state.localVelocity(), + robot_state.angularVelocity()); } else { @@ -367,23 +367,14 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { SSLSimulationProto::RobotControl robot_control; - auto sim_state = getSimulatorState(); - std::map> current_velocity_map; - if (side == gameController::Team::BLUE) - { - const auto& sim_robots = sim_state.blue_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); - } - else - { - const auto& sim_robots = sim_state.yellow_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); - } + auto sim_state = getSimulatorState(); + const auto& sim_robots = (side == gameController::Team::BLUE) + ? sim_state.blue_robots() + : sim_state.yellow_robots(); + const auto robot_map = getRobotIdToRobotStateMap(sim_robots); - for (auto& primitive_executor_with_id : robot_primitive_executor_map) + for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map) { - unsigned int robot_id = primitive_executor_with_id.first; - auto& primitive_executor = primitive_executor_with_id.second; std::unique_ptr direct_control; TbotsProto::PrimitiveExecutorStatus status; // Added for compilation @@ -391,10 +382,10 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { auto direct_control_no_ramp = primitive_executor->stepPrimitive(status, primitive_executor_time_step); - direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, - primitive_executor_time_step); + const auto& robot_state = robot_map.at(robot_id); + direct_control = getRampedVelocityPrimitive( + robot_state.localVelocity(), robot_state.angularVelocity(), + *direct_control_no_ramp, primitive_executor_time_step); } else { @@ -563,18 +554,18 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } -std::map> -ErForceSimulator::getRobotIdToLocalVelocityMap( +std::map ErForceSimulator::getRobotIdToRobotStateMap( const google::protobuf::RepeatedPtrField& sim_robots) { - std::map> robot_to_local_velocity; + std::map robot_map; for (const auto& sim_robot : sim_robots) { - const Vector local_vel = - globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()), - Angle::fromRadians(sim_robot.angle())); - const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); - robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel}; + const Point position = Point(sim_robot.p_x(), sim_robot.p_y()); + const Vector velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); + const Angle orientation = Angle::fromRadians(sim_robot.angle()); + const AngularVelocity angular_velocity = Angle::fromRadians(sim_robot.r_z()); + robot_map[sim_robot.id()] = + RobotState(position, velocity, orientation, angular_velocity); } - return robot_to_local_velocity; + return robot_map; } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 429b16d503..19a6f16fd6 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -7,6 +7,7 @@ #include "software/embedded/primitive_executor.h" #include "software/physics/euclidean_to_wheel.h" #include "software/world/field.h" +#include "software/world/robot_state.h" #include "software/world/team_types.h" #include "software/world/world.h" @@ -158,8 +159,7 @@ class ErForceSimulator RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity); + const TbotsProto::World& world_msg, const RobotState& robot_state); /** * Gets a map from robot id to local and angular velocity from repeated sim robots @@ -168,8 +168,7 @@ class ErForceSimulator * * @return a map from robot id to local velocity and angular velocity */ - static std::map> - getRobotIdToLocalVelocityMap( + static std::map getRobotIdToRobotStateMap( const google::protobuf::RepeatedPtrField& sim_robots); /** diff --git a/src/software/world/BUILD b/src/software/world/BUILD index 96693b306f..b06e1a0d92 100644 --- a/src/software/world/BUILD +++ b/src/software/world/BUILD @@ -139,6 +139,7 @@ cc_library( "//software/geom:angular_velocity", "//software/geom:point", "//software/geom:vector", + "//software/physics:velocity_conversion_util", "//software/time:duration", ], ) diff --git a/src/software/world/robot_state.cpp b/src/software/world/robot_state.cpp index c1f1266622..0e8fa01cad 100644 --- a/src/software/world/robot_state.cpp +++ b/src/software/world/robot_state.cpp @@ -1,5 +1,7 @@ #include "software/world/robot_state.h" +#include "software/physics/velocity_conversion_util.h" + RobotState::RobotState(const Point& position, const Vector& velocity, const Angle& orientation, const AngularVelocity& angular_velocity, const bool breakbeam_tripped) @@ -33,6 +35,11 @@ Vector RobotState::velocity() const return velocity_; } +Vector RobotState::localVelocity() const +{ + return globalToLocalVelocity(velocity_, orientation_); +} + Angle RobotState::orientation() const { return orientation_; diff --git a/src/software/world/robot_state.h b/src/software/world/robot_state.h index fac609fa12..d4832badab 100644 --- a/src/software/world/robot_state.h +++ b/src/software/world/robot_state.h @@ -59,6 +59,13 @@ class RobotState */ Vector velocity() const; + /** + * Returns the local velocity of the robot represented by this state + * + * @return the local velocity of the robot represented by this state + */ + Vector localVelocity() const; + /** * Returns the orientation of the robot represented by this state * From b5b513be639dfb14f81256bdaaf5f1d61545a2e5 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 16:22:42 -0700 Subject: [PATCH 09/18] Prim exec update velocity to update state (same logic as only update velocity) --- src/software/embedded/primitive_executor.cpp | 7 +++---- src/software/embedded/primitive_executor.h | 8 +++----- src/software/embedded/thunderloop.cpp | 7 ++++--- src/software/simulation/er_force_simulator.cpp | 3 +-- 4 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index a64262f3dc..2859bc6c87 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -34,13 +34,12 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m } } -void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity) +void PrimitiveExecutor::updateState(const RobotState& state) { Vector actual_global_velocity = - localToGlobalVelocity(local_velocity, state_.orientation()); + localToGlobalVelocity(state.localVelocity(), state_.orientation()); state_.setVelocity(actual_global_velocity); - state_.setAngularVelocity(angular_velocity); + state_.setAngularVelocity(state.angularVelocity()); } Vector PrimitiveExecutor::getTargetLinearVelocity() diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 8a563b92f1..df081b321d 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -26,13 +26,11 @@ class PrimitiveExecutor void updatePrimitive(const TbotsProto::Primitive& primitive_msg); /** - * Update primitive executor with the current velocity of the robot + * Update primitive executor with the state of the robot * - * @param local_velocity The current _local_ velocity - * @param angular_velocity The current angular velocity + * @param state The current robot state */ - void updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity); + void updateState(const RobotState& state); /** * Steps the current primitive and returns a direct control primitive with the diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 01445023b0..3e78cd51af 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -18,6 +18,7 @@ #include "software/networking/tbots_network_exception.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" +#include "software/world/robot_state.h" /** * https://web.archive.org/web/20210308013218/https://rt.wiki.kernel.org/index.php/Squarewave-example @@ -262,9 +263,9 @@ void Thunderloop::runLoop() if (motor_status_.has_value()) { auto status = motor_status_.value(); - primitive_executor_.updateVelocity( - createVector(status.local_velocity()), - createAngularVelocity(status.angular_velocity())); + primitive_executor_.updateState( + RobotState(Point(), createVector(status.local_velocity()), Angle(), + createAngularVelocity(status.angular_velocity()))); } // Timeout Overrides for Primitives diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index ec7b69cf23..9537cc8118 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -350,8 +350,7 @@ void ErForceSimulator::setRobotPrimitive( { auto primitive_executor = robot_primitive_executor_iter->second; primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id)); - primitive_executor->updateVelocity(robot_state.localVelocity(), - robot_state.angularVelocity()); + primitive_executor->updateState(robot_state); } else { From 5403bbfb6ff80512df6e1aaa1be585d6b8f3d685 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 16:39:15 -0700 Subject: [PATCH 10/18] Use auto --- src/software/simulation/er_force_simulator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 9537cc8118..41d97a6153 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -559,10 +559,10 @@ std::map ErForceSimulator::getRobotIdToRobotStateMap( std::map robot_map; for (const auto& sim_robot : sim_robots) { - const Point position = Point(sim_robot.p_x(), sim_robot.p_y()); - const Vector velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); - const Angle orientation = Angle::fromRadians(sim_robot.angle()); - const AngularVelocity angular_velocity = Angle::fromRadians(sim_robot.r_z()); + const auto position = Point(sim_robot.p_x(), sim_robot.p_y()); + const auto velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); + const auto orientation = Angle::fromRadians(sim_robot.angle()); + const auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z()); robot_map[sim_robot.id()] = RobotState(position, velocity, orientation, angular_velocity); } From 83f511343292accba3ad7f69e031d8894c7d9e50 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 16:49:01 -0700 Subject: [PATCH 11/18] Separate time since trajectory creation variables --- src/software/embedded/primitive_executor.cpp | 20 ++++++++++++-------- src/software/embedded/primitive_executor.h | 5 ++++- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 2859bc6c87..c37edc3c91 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -30,7 +30,10 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), state_.angularVelocity(), robot_constants_); - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + time_since_linear_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + time_since_angular_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); } } @@ -45,10 +48,10 @@ void PrimitiveExecutor::updateState(const RobotState& state) Vector PrimitiveExecutor::getTargetLinearVelocity() { Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), + trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.toSeconds()), state_.orientation()); Point position = - trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); + trajectory_path_->getPosition(time_since_linear_trajectory_creation_.toSeconds()); double distance_to_destination = distance(position, trajectory_path_->getDestination()); @@ -62,11 +65,11 @@ Vector PrimitiveExecutor::getTargetLinearVelocity() AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() { - state_.setOrientation( - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds())); + state_.setOrientation(angular_trajectory_->getPosition( + time_since_angular_trajectory_creation_.toSeconds())); - AngularVelocity angular_velocity = - angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); + AngularVelocity angular_velocity = angular_trajectory_->getVelocity( + time_since_angular_trajectory_creation_.toSeconds()); Angle orientation_to_destination = state_.orientation().minDiff(angular_trajectory_->getDestination()); if (orientation_to_destination.toDegrees() < 5) @@ -81,7 +84,8 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time) { - time_since_trajectory_creation_ += delta_time; + time_since_linear_trajectory_creation_ += delta_time; + time_since_angular_trajectory_creation_ += delta_time; status.set_running_primitive(true); switch (current_primitive_.primitive_case()) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index df081b321d..10d4337b5f 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -61,11 +61,14 @@ class PrimitiveExecutor RobotState state_; TbotsProto::Primitive current_primitive_; - Duration time_since_trajectory_creation_; robot_constants::RobotConstants robot_constants_; + std::optional trajectory_path_; std::optional angular_trajectory_; + Duration time_since_linear_trajectory_creation_; + Duration time_since_angular_trajectory_creation_; + // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; From 7126c14faea5271a9494940ff8d8163049bc1b97 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 18:52:52 -0700 Subject: [PATCH 12/18] Port trajectory path changes from robot localizer branch --- .../message_translation/tbots_protobuf.cpp | 16 ++++++++-------- src/proto/message_translation/tbots_protobuf.h | 11 +++++++---- .../message_translation/tbots_protobuf_test.cpp | 4 ++-- src/software/ai/navigator/trajectory/BUILD | 1 + .../trajectory/bang_bang_trajectory_1d.h | 15 +++++++++++++++ .../bang_bang_trajectory_1d_angular.h | 17 +++++++++++++++++ .../ai/navigator/trajectory/trajectory.hpp | 12 ++++++++++++ .../ai/navigator/trajectory/trajectory_2d.h | 17 +++++++++++++++++ 8 files changed, 79 insertions(+), 14 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..2ba0c2c6b9 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -425,7 +425,8 @@ std::unique_ptr createCostVisualization( } std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants) { double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed( @@ -449,8 +450,7 @@ std::optional createTrajectoryPathFromParams( } auto trajectory = std::make_shared( - createPoint(params.start_position()), initial_destination, initial_velocity, - constraints); + start_position, initial_destination, initial_velocity, constraints); TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator); @@ -475,14 +475,14 @@ std::optional createTrajectoryPathFromParams( } BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants) { return BangBangTrajectory1DAngular( - createAngle(params.start_angle()), createAngle(params.final_angle()), - initial_velocity, - AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s), + start_angle, createAngle(params.final_angle()), initial_velocity, + AngularVelocity::fromRadians( + robot_constants.robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_m_per_s; + return robot_constants.robot_trajectory_max_speed_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 5976da5fcc..2831857e02 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -238,25 +238,28 @@ std::unique_ptr createCostVisualization( * Generate a 2D Trajectory Path given 2D trajectory parameters * * @param params 2D Trajectory Path + * @param start_position Initial position to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return TrajectoryPath, or std::nullopt if the trajectory path could not be created * from the given parameters */ std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants); /** - * Generate an angular trajectory Path given angular trajectory proto parameters + * Generate an angular trajectory path given angular trajectory proto parameters * - * @param params angular Trajectory Path + * @param params Angular trajectory path + * @param start_angle Initial angle to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return Generate angular trajectory */ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0f82de69d6..2735d57a3b 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.add_sub_destinations()) = sub_destination_proto; } - auto converted_trajectory_path_opt = - createTrajectoryPathFromParams(params, initial_velocity, robot_constants); + auto converted_trajectory_path_opt = createTrajectoryPathFromParams( + params, createPoint(params.start_position()), initial_velocity, robot_constants); ASSERT_TRUE(converted_trajectory_path_opt.has_value()); TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value(); diff --git a/src/software/ai/navigator/trajectory/BUILD b/src/software/ai/navigator/trajectory/BUILD index abc4f5c82f..5b2ef42fb2 100644 --- a/src/software/ai/navigator/trajectory/BUILD +++ b/src/software/ai/navigator/trajectory/BUILD @@ -21,6 +21,7 @@ cc_library( deps = [ ":trajectory", "//software/geom:point", + "//software/geom/algorithms", ], ) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index d5a54bf7af..21f5fe882a 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -103,6 +103,21 @@ class BangBangTrajectory1D : public Trajectory */ size_t getNumTrajectoryParts() const; + /** + * Checks if this 1D trajectory terminates at approximately the same destination + * position as another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance between the two destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + bool hasSameDestination(const Trajectory& other, + double threshold) const override + { + return std::abs(getDestination() - other.getDestination()) <= threshold; + } + private: /** * Generate a trapezoidal trajectory and fill in `trajectory_parts`. diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 2ffbff5ec2..dab2db5dc8 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -83,6 +83,23 @@ class BangBangTrajectory1DAngular */ double getTotalTime() const override; + /** + * Checks if this angular trajectory terminates at approximately the same destination + * orientation as another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance in radians between the two + * destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + bool hasSameDestination( + const Trajectory& other, + double threshold) const override + { + return getDestination().minDiff(other.getDestination()).toDegrees() <= threshold; + } + private: BangBangTrajectory1D trajectory; }; diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 4e7a7ee77e..c4765b129a 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -59,4 +59,16 @@ class Trajectory { return getPosition(getTotalTime()); } + + /** + * Checks if this trajectory terminates at approximately the same destination as + * another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance between the two destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + virtual bool hasSameDestination(const Trajectory& other, + double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index 9699f07c17..8f2485587b 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -1,6 +1,7 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory.hpp" +#include "software/geom/algorithms/distance.h" #include "software/geom/point.h" #include "software/geom/rectangle.h" @@ -14,4 +15,20 @@ class Trajectory2D : virtual public Trajectory * @return bounding boxes which this trajectory passes through */ virtual std::vector getBoundingBoxes() const = 0; + + /** + * Checks if this 2D trajectory terminates at approximately the same destination + * position as another trajectory. + * + * @param other The other trajectory to compare against. + * @param threshold The maximum allowable distance in metres between the two + * destinations. + * @return True if the distance between destinations is less than or equal to the + * threshold. False otherwise. + */ + bool hasSameDestination(const Trajectory& other, + double threshold) const override + { + return distance(getDestination(), other.getDestination()) <= threshold; + } }; From 1d6c8e701fc60ff6cf9d3abc197c8207ab22bafa Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 19:39:21 -0700 Subject: [PATCH 13/18] Update robot constants --- .../message_translation/tbots_protobuf.cpp | 2 +- src/shared/robot_constants.h | 33 +++++++++++++------ 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 2ba0c2c6b9..f31737c17f 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -482,7 +482,7 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( return BangBangTrajectory1DAngular( start_angle, createAngle(params.final_angle()), initial_velocity, AngularVelocity::fromRadians( - robot_constants.robot_max_ang_speed_trajectory_rad_per_s), + robot_constants.robot_trajectory_max_ang_speed_rad_per_s), AngularVelocity::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 00a77c94da..5610133965 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -88,6 +88,10 @@ struct RobotConstants // The maximum speed achievable by our robots, in metres per second [m/s] float robot_max_speed_m_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] + float robot_trajectory_max_speed_m_per_s; + // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -97,6 +101,11 @@ struct RobotConstants // The maximum angular speed achievable by our robots [rad/s] float robot_max_ang_speed_rad_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. + // [rad/s] + float robot_trajectory_max_ang_speed_rad_per_s; + // The maximum angular acceleration achievable by our robots [rad/s^2] float robot_max_ang_acceleration_rad_per_s_2; @@ -142,13 +151,15 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 2.0f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.0f, + .robot_trajectory_max_speed_m_per_s = 3.0f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_trajectory_max_ang_speed_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f, @@ -173,13 +184,15 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.000f, + .robot_trajectory_max_speed_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_trajectory_max_ang_speed_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f}; } From b00e31ffd5c8fcfb3cf281e3e6e5ec2d112e4b3b Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 19:46:38 -0700 Subject: [PATCH 14/18] Add cmath dependency --- src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index 21f5fe882a..bb779c2996 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "software/ai/navigator/trajectory/trajectory.hpp" From c855e54af94e9d52c977382e4d32b982e114167f Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Thu, 4 Jun 2026 12:25:47 -0700 Subject: [PATCH 15/18] Implement getting velocities from motion controllers --- src/software/embedded/BUILD | 2 + .../embedded/motion_control/controller.h | 2 + src/software/embedded/primitive_executor.cpp | 105 ++++++++++-------- src/software/embedded/primitive_executor.h | 21 +++- 4 files changed, 81 insertions(+), 49 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index e5bba4bd99..44f22ed099 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -34,6 +34,8 @@ cc_library( "//proto/primitive:primitive_msg_factory", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", + "//software/embedded/motion_control:orientation_controller", + "//software/embedded/motion_control:position_controller", "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", diff --git a/src/software/embedded/motion_control/controller.h b/src/software/embedded/motion_control/controller.h index 6de0d30237..6882618082 100644 --- a/src/software/embedded/motion_control/controller.h +++ b/src/software/embedded/motion_control/controller.h @@ -1,3 +1,5 @@ +#pragma once + #include "software/time/duration.h" /** diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index c37edc3c91..2b9a5dc189 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -22,62 +22,77 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (current_primitive_.has_move()) { - trajectory_path_ = - createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), - state_.velocity(), robot_constants_); - - angular_trajectory_ = - createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - state_.angularVelocity(), robot_constants_); - - time_since_linear_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - time_since_angular_trajectory_creation_ = - Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + const auto new_trajectory_path = createTrajectoryPathFromParams( + current_primitive_.move().xy_traj_params(), state_.position(), + state_.velocity(), robot_constants_); + + const auto new_angular_trajectory = createAngularTrajectoryFromParams( + current_primitive_.move().w_traj_params(), state_.orientation(), + state_.angularVelocity(), robot_constants_); + + const bool is_linear_trajectory_new = + new_trajectory_path.has_value() != trajectory_path_.has_value() || + (trajectory_path_.has_value() && + !trajectory_path_->hasSameDestination(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); + + const bool is_angular_trajectory_new = + !angular_trajectory_.has_value() || + !angular_trajectory_->hasSameDestination( + new_angular_trajectory, ANGULAR_DESTINATION_THRESHOLD_DEGREES); + + if (is_linear_trajectory_new) + { + trajectory_path_ = new_trajectory_path; + position_controller_.reset(); + time_since_linear_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + } + + if (is_angular_trajectory_new) + { + angular_trajectory_ = new_angular_trajectory; + orientation_controller_.reset(); + time_since_angular_trajectory_creation_ = + Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + } } } void PrimitiveExecutor::updateState(const RobotState& state) { - Vector actual_global_velocity = - localToGlobalVelocity(state.localVelocity(), state_.orientation()); - state_.setVelocity(actual_global_velocity); - state_.setAngularVelocity(state.angularVelocity()); + state_ = state; } -Vector PrimitiveExecutor::getTargetLinearVelocity() +Vector PrimitiveExecutor::stepTargetLinearVelocity(Duration delta_time) { - Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_linear_trajectory_creation_.toSeconds()), - state_.orientation()); - Point position = - trajectory_path_->getPosition(time_since_linear_trajectory_creation_.toSeconds()); - double distance_to_destination = - distance(position, trajectory_path_->getDestination()); - - // Dampen velocity as we get closer to the destination to reduce jittering - if (distance_to_destination < MAX_DAMPENING_VELOCITY_DISTANCE_M) - { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; - } - return local_velocity; + const auto target_v_global = + position_controller_.step(state_.position(), *trajectory_path_, + time_since_linear_trajectory_creation_, delta_time); + const auto target_v_local = + globalToLocalVelocity(target_v_global, state_.orientation()); + + return target_v_local.normalize( + std::min(target_v_local.length(), + static_cast(robot_constants_.robot_max_speed_m_per_s))); } -AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() +AngularVelocity PrimitiveExecutor::stepTargetAngularVelocity(Duration delta_time) { - state_.setOrientation(angular_trajectory_->getPosition( - time_since_angular_trajectory_creation_.toSeconds())); - - AngularVelocity angular_velocity = angular_trajectory_->getVelocity( - time_since_angular_trajectory_creation_.toSeconds()); - Angle orientation_to_destination = - state_.orientation().minDiff(angular_trajectory_->getDestination()); - if (orientation_to_destination.toDegrees() < 5) + const auto target_w = + orientation_controller_.step(state_.orientation(), *angular_trajectory_, + time_since_angular_trajectory_creation_, delta_time); + if (target_w.toRadians() < -robot_constants_.robot_max_ang_speed_rad_per_s) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; + return AngularVelocity::fromRadians( + -robot_constants_.robot_max_ang_speed_rad_per_s); } - - return angular_velocity; + if (target_w.toRadians() > robot_constants_.robot_max_ang_speed_rad_per_s) + { + return AngularVelocity::fromRadians( + robot_constants_.robot_max_ang_speed_rad_per_s); + } + return target_w; } @@ -117,8 +132,8 @@ std::unique_ptr PrimitiveExecutor::stepPrimi return output; } - Vector local_velocity = getTargetLinearVelocity(); - AngularVelocity angular_velocity = getTargetAngularVelocity(); + Vector local_velocity = stepTargetLinearVelocity(delta_time); + AngularVelocity angular_velocity = stepTargetAngularVelocity(delta_time); auto output = createDirectControlPrimitive( local_velocity, angular_velocity, diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 10d4337b5f..51a3228505 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -4,6 +4,8 @@ #include "proto/tbots_software_msgs.pb.h" #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" +#include "software/embedded/motion_control/orientation_controller.h" +#include "software/embedded/motion_control/position_controller.h" #include "software/geom/vector.h" #include "software/time/duration.h" #include "software/world/robot_state.h" @@ -47,17 +49,20 @@ class PrimitiveExecutor private: /* - * Compute the next target linear _local_ velocity the robot should be at. + * Compute the next target linear _local_ velocity the robot should have. + * @param delta_time The elapsed time since last time step + * * @returns Vector The target linear _local_ velocity */ - Vector getTargetLinearVelocity(); + Vector stepTargetLinearVelocity(Duration delta_time); /* - * Returns the next target angular velocity the robot + * Compute the next target angular velocity the robot should have. + * @param delta_time The elapsed time since last time step * * @returns AngularVelocity The target angular velocity */ - AngularVelocity getTargetAngularVelocity(); + AngularVelocity stepTargetAngularVelocity(Duration delta_time); RobotState state_; TbotsProto::Primitive current_primitive_; @@ -69,10 +74,18 @@ class PrimitiveExecutor Duration time_since_linear_trajectory_creation_; Duration time_since_angular_trajectory_creation_; + PositionController position_controller_; + OrientationController orientation_controller_; + // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; // The distance away from the destination at which we start dampening the velocity // to avoid jittering around the destination. static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; + + // If distance between current linear trajectory destination and new one is larger + // than this, we change trajectories. + static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; + static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; }; From 48eaee340dbc09c2a271a8dca604230f0d81f6d3 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Thu, 4 Jun 2026 14:04:30 -0700 Subject: [PATCH 16/18] Fix for inverted yellow team world --- .../simulation/er_force_simulator.cpp | 29 ++++++++++++++----- src/software/simulation/er_force_simulator.h | 5 +++- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 41d97a6153..c501d61b25 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -299,7 +299,8 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( { auto sim_state = getSimulatorState(); const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_map = getRobotIdToRobotStateMap(sim_robots); + const auto robot_map = + getRobotIdToRobotStateMap(sim_robots, gameController::Team::YELLOW); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; @@ -320,7 +321,8 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( { auto sim_state = getSimulatorState(); const auto& sim_robots = sim_state.blue_robots(); - const auto robot_map = getRobotIdToRobotStateMap(sim_robots); + const auto robot_map = + getRobotIdToRobotStateMap(sim_robots, gameController::Team::BLUE); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; @@ -370,7 +372,7 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( const auto& sim_robots = (side == gameController::Team::BLUE) ? sim_state.blue_robots() : sim_state.yellow_robots(); - const auto robot_map = getRobotIdToRobotStateMap(sim_robots); + const auto robot_map = getRobotIdToRobotStateMap(sim_robots, side); for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map) { @@ -554,15 +556,26 @@ void ErForceSimulator::resetCurrentTime() } std::map ErForceSimulator::getRobotIdToRobotStateMap( - const google::protobuf::RepeatedPtrField& sim_robots) + const google::protobuf::RepeatedPtrField& sim_robots, + gameController::Team side) { std::map robot_map; for (const auto& sim_robot : sim_robots) { - const auto position = Point(sim_robot.p_x(), sim_robot.p_y()); - const auto velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); - const auto orientation = Angle::fromRadians(sim_robot.angle()); - const auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z()); + auto position = Point(sim_robot.p_x(), sim_robot.p_y()); + auto velocity = Vector(sim_robot.v_x(), sim_robot.v_y()); + auto orientation = Angle::fromRadians(sim_robot.angle()); + auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z()); + + // TODO(...): actually fix root issue + if (side == gameController::Team::YELLOW) + { + position = -position; + velocity = -velocity; + orientation += Angle::half(); + // angular_velocity is the same no matter which side + } + robot_map[sim_robot.id()] = RobotState(position, velocity, orientation, angular_velocity); } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 19a6f16fd6..79118fc25c 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -165,11 +165,14 @@ class ErForceSimulator * Gets a map from robot id to local and angular velocity from repeated sim robots * * @param sim_robots Repeated er force sim robot protos + * @param side Which team the robots belong to. If team is yellow, inverts position + * and linear velocity, and adds 180 degrees to orientation. * * @return a map from robot id to local velocity and angular velocity */ static std::map getRobotIdToRobotStateMap( - const google::protobuf::RepeatedPtrField& sim_robots); + const google::protobuf::RepeatedPtrField& sim_robots, + gameController::Team side); /** * Update Simulator Robot and get the latest robot control From b2800955504bf277b8145474539e9bd817bae4f6 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Thu, 4 Jun 2026 15:32:43 -0700 Subject: [PATCH 17/18] Remove TODO comment --- src/software/simulation/er_force_simulator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index c501d61b25..93bbed7525 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -567,7 +567,6 @@ std::map ErForceSimulator::getRobotIdToRobotStateMap( auto orientation = Angle::fromRadians(sim_robot.angle()); auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z()); - // TODO(...): actually fix root issue if (side == gameController::Team::YELLOW) { position = -position; From b870c793ff8c5f91c12ca4171d8371f446071ad2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 5 Jun 2026 04:57:35 +0000 Subject: [PATCH 18/18] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/thunderloop.cpp | 4 ++-- src/software/simulation/er_force_simulator.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 3e78cd51af..15916a04fe 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -287,8 +287,8 @@ void Thunderloop::runLoop() primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } - direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); + direct_control_ = *primitive_executor_.stepPrimitive( + primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); } thunderloop_status_.set_primitive_executor_step_time_ms( diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 93bbed7525..d45661af8f 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -385,8 +385,8 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( primitive_executor->stepPrimitive(status, primitive_executor_time_step); const auto& robot_state = robot_map.at(robot_id); direct_control = getRampedVelocityPrimitive( - robot_state.localVelocity(), robot_state.angularVelocity(), - *direct_control_no_ramp, primitive_executor_time_step); + robot_state.localVelocity(), robot_state.angularVelocity(), + *direct_control_no_ramp, primitive_executor_time_step); } else {