Use RobotState instead of individual variables for prim exec (#3759)

nycrat · web-flow · commit 2047239e0603 · 2026-06-04T21:49:29.000-07:00
* Remove unused member functions and variables

* Fix call to setStopPrimitive

this is removed even thought is technically used now cause future
changes will get rid of it

* Pass in delta_time dynamically for prim exec

* Fix calls to prim exec constructor and stepPrimitive

* Add setters to RobotState class

* Replace state variables with one RobotState

* Create default constructor for robot state

* Replace use of std::pair with RobotState

* Prim exec update velocity to update state (same logic as only update velocity)

* Use auto

* Formatting
diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp
@@ -22,30 +22,31 @@ 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);
     }
 }
 
-void PrimitiveExecutor::updateVelocity(const Vector& local_velocity,
-                                       const AngularVelocity& angular_velocity)
+void PrimitiveExecutor::updateState(const RobotState& state)
 {
-    Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_);
-    velocity_                     = actual_global_velocity;
-    angular_velocity_             = angular_velocity;
+    Vector actual_global_velocity =
+        localToGlobalVelocity(state.localVelocity(), state_.orientation());
+    state_.setVelocity(actual_global_velocity);
+    state_.setAngularVelocity(state.angularVelocity());
 }
 
 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 +62,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
@@ -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
@@ -61,11 +59,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<TrajectoryPath> trajectory_path_;
     std::optional<BangBangTrajectory1DAngular> angular_trajectory_;
diff --git 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
@@ -297,19 +297,19 @@ void ErForceSimulator::setYellowRobotPrimitiveSet(
     const TbotsProto::PrimitiveSet& primitive_set_msg,
     std::unique_ptr<TbotsProto::World> 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<TbotsProto::World> 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<unsigned int, std::shared_ptr<PrimitiveExecutor>>&
         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,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(local_velocity, angular_velocity);
+        primitive_executor->updateState(robot_state);
     }
     else
     {
@@ -367,34 +366,25 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots(
 {
     SSLSimulationProto::RobotControl robot_control;
 
-    auto sim_state = getSimulatorState();
-    std::map<RobotId, std::pair<Vector, Angle>> 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<TbotsProto::DirectControlPrimitive> direct_control;
 
         TbotsProto::PrimitiveExecutorStatus status;  // Added for compilation
         if (ramping)
         {
             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 +553,18 @@ void ErForceSimulator::resetCurrentTime()
     current_time = Timestamp::fromSeconds(0);
 }
 
-std::map<RobotId, std::pair<Vector, AngularVelocity>>
-ErForceSimulator::getRobotIdToLocalVelocityMap(
+std::map<RobotId, RobotState> ErForceSimulator::getRobotIdToRobotStateMap(
     const google::protobuf::RepeatedPtrField<world::SimRobot>& sim_robots)
 {
-    std::map<RobotId, std::pair<Vector, AngularVelocity>> robot_to_local_velocity;
+    std::map<RobotId, RobotState> 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 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);
     }
-    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
@@ -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<unsigned int, std::shared_ptr<PrimitiveExecutor>>&
             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<RobotId, std::pair<Vector, AngularVelocity>>
-    getRobotIdToLocalVelocityMap(
+    static std::map<RobotId, RobotState> getRobotIdToRobotStateMap(
         const google::protobuf::RepeatedPtrField<world::SimRobot>& sim_robots);
 
     /**
diff --git 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
@@ -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_;
@@ -48,6 +55,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
@@ -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
@@ -54,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
      *
@@ -75,6 +87,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

Original file line number	Diff line number	Diff line change
`@@ -297,19 +297,19 @@ void ErForceSimulator::setYellowRobotPrimitiveSet(`
`297`	`297`	`const TbotsProto::PrimitiveSet& primitive_set_msg,`
`298`	`298`	`std::unique_ptr<TbotsProto::World> world_msg)`
`299`	`299`	`{`
`300`		`- auto sim_state = getSimulatorState();`
`301`		`- const auto& sim_robots = sim_state.yellow_robots();`
`302`		`- const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots);`
	`300`	`+ auto sim_state = getSimulatorState();`
	`301`	`+ const auto& sim_robots = sim_state.yellow_robots();`
	`302`	`+ const auto robot_map = getRobotIdToRobotStateMap(sim_robots);`
`303`	`303`
`304`	`304`	`yellow_team_world_msg = std::move(world_msg);`
`305`	`305`	`const TbotsProto::World world_proto = *yellow_team_world_msg;`
`306`	`306`	`for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives())`
`307`	`307`	`{`
`308`		`- if (robot_to_vel_pair_map.contains(robot_id))`
	`308`	`+ if (robot_map.contains(robot_id))`
`309`	`309`	`{`
`310`		`- auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id);`
	`310`	`+ const auto& robot_state = robot_map.at(robot_id);`
`311`	`311`	`setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map,`
`312`		`- world_proto, local_vel, angular_vel);`
	`312`	`+ world_proto, robot_state);`
`313`	`313`	`}`
`314`	`314`	`}`
`315`	`315`	`}`
`@@ -318,20 +318,20 @@ void ErForceSimulator::setBlueRobotPrimitiveSet(`
`318`	`318`	`const TbotsProto::PrimitiveSet& primitive_set_msg,`
`319`	`319`	`std::unique_ptr<TbotsProto::World> world_msg)`
`320`	`320`	`{`
`321`		`- auto sim_state = getSimulatorState();`
`322`		`- const auto& sim_robots = sim_state.blue_robots();`
`323`		`- const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots);`
	`321`	`+ auto sim_state = getSimulatorState();`
	`322`	`+ const auto& sim_robots = sim_state.blue_robots();`
	`323`	`+ const auto robot_map = getRobotIdToRobotStateMap(sim_robots);`
`324`	`324`
`325`	`325`	`blue_team_world_msg = std::move(world_msg);`
`326`	`326`	`const TbotsProto::World world_proto = *blue_team_world_msg;`
`327`	`327`
`328`	`328`	`for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives())`
`329`	`329`	`{`
`330`		`- if (robot_to_vel_pair_map.contains(robot_id))`
	`330`	`+ if (robot_map.contains(robot_id))`
`331`	`331`	`{`
`332`		`- auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id);`
	`332`	`+ const auto& robot_state = robot_map.at(robot_id);`
`333`	`333`	`setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map,`
`334`		`- world_proto, local_vel, angular_vel);`
	`334`	`+ world_proto, robot_state);`
`335`	`335`	`}`
`336`	`336`	`}`
`337`	`337`	`}`
`@@ -340,8 +340,7 @@ void ErForceSimulator::setRobotPrimitive(`
`340`	`340`	`RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg,`
`341`	`341`	`std::unordered_map<unsigned int, std::shared_ptr<PrimitiveExecutor>>&`
`342`	`342`	`robot_primitive_executor_map,`
`343`		`- const TbotsProto::World& world_msg, const Vector& local_velocity,`
`344`		`- const AngularVelocity angular_velocity)`
	`343`	`+ const TbotsProto::World& world_msg, const RobotState& robot_state)`
`345`	`344`	`{`
`346`	`345`	`// Set to NEG_X because the world msg in this simulator is normalized`
`347`	`346`	`// correctly`
`@@ -351,7 +350,7 @@ void ErForceSimulator::setRobotPrimitive(`
`351`	`350`	`{`
`352`	`351`	`auto primitive_executor = robot_primitive_executor_iter->second;`
`353`	`352`	`primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id));`
`354`		`- primitive_executor->updateVelocity(local_velocity, angular_velocity);`
	`353`	`+ primitive_executor->updateState(robot_state);`
`355`	`354`	`}`
`356`	`355`	`else`
`357`	`356`	`{`
`@@ -367,34 +366,25 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots(`
`367`	`366`	`{`
`368`	`367`	`SSLSimulationProto::RobotControl robot_control;`
`369`	`368`
`370`		`- auto sim_state = getSimulatorState();`
`371`		`- std::map<RobotId, std::pair<Vector, Angle>> current_velocity_map;`
`372`		`- if (side == gameController::Team::BLUE)`
`373`		`- {`
`374`		`- const auto& sim_robots = sim_state.blue_robots();`
`375`		`- current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots);`
`376`		`- }`
`377`		`- else`
`378`		`- {`
`379`		`- const auto& sim_robots = sim_state.yellow_robots();`
`380`		`- current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots);`
`381`		`- }`
	`369`	`+ auto sim_state = getSimulatorState();`
	`370`	`+ const auto& sim_robots = (side == gameController::Team::BLUE)`
	`371`	`+ ? sim_state.blue_robots()`
	`372`	`+ : sim_state.yellow_robots();`
	`373`	`+ const auto robot_map = getRobotIdToRobotStateMap(sim_robots);`
`382`	`374`
`383`		`- for (auto& primitive_executor_with_id : robot_primitive_executor_map)`
	`375`	`+ for (auto& [robot_id, primitive_executor] : robot_primitive_executor_map)`
`384`	`376`	`{`
`385`		`- unsigned int robot_id = primitive_executor_with_id.first;`
`386`		`- auto& primitive_executor = primitive_executor_with_id.second;`
`387`	`377`	`std::unique_ptr<TbotsProto::DirectControlPrimitive> direct_control;`
`388`	`378`
`389`	`379`	`TbotsProto::PrimitiveExecutorStatus status; // Added for compilation`
`390`	`380`	`if (ramping)`
`391`	`381`	`{`
`392`	`382`	`auto direct_control_no_ramp =`
`393`	`383`	`primitive_executor->stepPrimitive(status, primitive_executor_time_step);`
`394`		`- direct_control = getRampedVelocityPrimitive(`
`395`		`- current_velocity_map.at(robot_id).first,`
`396`		`- current_velocity_map.at(robot_id).second, *direct_control_no_ramp,`
`397`		`- primitive_executor_time_step);`
	`384`	`+ const auto& robot_state = robot_map.at(robot_id);`
	`385`	`+ direct_control = getRampedVelocityPrimitive(`
	`386`	`+ robot_state.localVelocity(), robot_state.angularVelocity(),`
	`387`	`+ *direct_control_no_ramp, primitive_executor_time_step);`
`398`	`388`	`}`
`399`	`389`	`else`
`400`	`390`	`{`
`@@ -563,18 +553,18 @@ void ErForceSimulator::resetCurrentTime()`
`563`	`553`	`current_time = Timestamp::fromSeconds(0);`
`564`	`554`	`}`
`565`	`555`
`566`		`-std::map<RobotId, std::pair<Vector, AngularVelocity>>`
`567`		`-ErForceSimulator::getRobotIdToLocalVelocityMap(`
	`556`	`+std::map<RobotId, RobotState> ErForceSimulator::getRobotIdToRobotStateMap(`
`568`	`557`	`const google::protobuf::RepeatedPtrField<world::SimRobot>& sim_robots)`
`569`	`558`	`{`
`570`		`- std::map<RobotId, std::pair<Vector, AngularVelocity>> robot_to_local_velocity;`
	`559`	`+ std::map<RobotId, RobotState> robot_map;`
`571`	`560`	`for (const auto& sim_robot : sim_robots)`
`572`	`561`	`{`
`573`		`- const Vector local_vel =`
`574`		`- globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()),`
`575`		`- Angle::fromRadians(sim_robot.angle()));`
`576`		`- const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z());`
`577`		`- robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel};`
	`562`	`+ const auto position = Point(sim_robot.p_x(), sim_robot.p_y());`
	`563`	`+ const auto velocity = Vector(sim_robot.v_x(), sim_robot.v_y());`
	`564`	`+ const auto orientation = Angle::fromRadians(sim_robot.angle());`
	`565`	`+ const auto angular_velocity = AngularVelocity::fromRadians(sim_robot.r_z());`
	`566`	`+ robot_map[sim_robot.id()] =`
	`567`	`+ RobotState(position, velocity, orientation, angular_velocity);`
`578`	`568`	`}`
`579`		`- return robot_to_local_velocity;`
	`569`	`+ return robot_map;`
`580`	`570`	`}`
Original file line number	Diff line number	Diff line change
`@@ -139,6 +139,7 @@ cc_library(`
`139`	`139`	`"//software/geom:angular_velocity",`
`140`	`140`	`"//software/geom:point",`
`141`	`141`	`"//software/geom:vector",`
	`142`	`+ "//software/physics:velocity_conversion_util",`
`142`	`143`	`"//software/time:duration",`
`143`	`144`	`],`
`144`	`145`	`)`