Pass in delta_time dynamically for prim exec (#3758)

nycrat · williamckha · pre-commit-ci-lite[bot] · web-flow · commit ad63b4208709 · 2026-06-04T18:49:17.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

* [pre-commit.ci lite] apply automatic fixes

---------

Co-authored-by: William Ha &lt;60044853+williamckha@users.noreply.github.com&gt;
Co-authored-by: pre-commit-ci-lite[bot] &lt;117423508+pre-commit-ci-lite[bot]@users.noreply.github.com&gt;
diff --git 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<TbotsProto::DirectControlPrimitive> 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
@@ -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<TbotsProto::DirectControlPrimitive> stepPrimitive(
-        TbotsProto::PrimitiveExecutorStatus& status);
+        TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time);
 
    private:
     /*
@@ -70,10 +70,6 @@ class PrimitiveExecutor
     std::optional<TrajectoryPath> trajectory_path_;
     std::optional<BangBangTrajectory1DAngular> 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;
 
diff --git 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();
 
@@ -286,8 +286,8 @@ void Thunderloop::runLoop()
                     primitive_executor_.updatePrimitive(*createStopPrimitiveProto());
                 }
 
-                direct_control_ =
-                    *primitive_executor_.stepPrimitive(primitive_executor_status_);
+                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
@@ -20,10 +20,10 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type,
                                    const robot_constants::RobotConstants& robot_constants,
                                    std::unique_ptr<RealismConfigErForce>& realism_config,
                                    const bool ramping,
-                                   double primitive_executor_time_step)
+                                   Duration primitive_executor_time_step)
     : yellow_team_world_msg(std::make_unique<TbotsProto::World>()),
       blue_team_world_msg(std::make_unique<TbotsProto::World>()),
-      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<PrimitiveExecutor>(
-                Duration::fromSeconds(primitive_executor_time_step_s), robot_constants);
+            auto robot_primitive_executor =
+                std::make_shared<PrimitiveExecutor>(robot_constants);
             blue_primitive_executor_map.insert({id, robot_primitive_executor});
         }
         else
         {
-            auto robot_primitive_executor = std::make_shared<PrimitiveExecutor>(
-                Duration::fromSeconds(primitive_executor_time_step_s), robot_constants);
+            auto robot_primitive_executor =
+                std::make_shared<PrimitiveExecutor>(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<TbotsProto::DirectControlPrimitive>
 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
@@ -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<RealismConfigErForce>& 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<RealismConfigErForce>& 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<unsigned int, std::shared_ptr<PrimitiveExecutor>>
@@ -212,7 +212,7 @@ class ErForceSimulator
     std::unique_ptr<TbotsProto::World> yellow_team_world_msg;
     std::unique_ptr<TbotsProto::World> blue_team_world_msg;
 
-    double primitive_executor_time_step_s;
+    Duration primitive_executor_time_step;
     unsigned int frame_number;
 
     // The current time.

Original file line number	Diff line number	Diff line change
`@@ -11,8 +11,8 @@`
`11`	`11`	`#include "software/physics/velocity_conversion_util.h"`
`12`	`12`
`13`	`13`	`PrimitiveExecutor::PrimitiveExecutor(`
`14`		`- const Duration time_step, const robot_constants::RobotConstants& robot_constants)`
`15`		`- : current_primitive_(), robot_constants_(robot_constants), time_step_(time_step)`
	`14`	`+ const robot_constants::RobotConstants& robot_constants)`
	`15`	`+ : current_primitive_(), robot_constants_(robot_constants)`
`16`	`16`	`{`
`17`	`17`	`}`
`18`	`18`
`@@ -78,9 +78,9 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity()`
`78`	`78`
`79`	`79`
`80`	`80`	`std::unique_ptr<TbotsProto::DirectControlPrimitive> PrimitiveExecutor::stepPrimitive(`
`81`		`- TbotsProto::PrimitiveExecutorStatus& status)`
	`81`	`+ TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time)`
`82`	`82`	`{`
`83`		`- time_since_trajectory_creation_ += time_step_;`
	`83`	`+ time_since_trajectory_creation_ += delta_time;`
`84`	`84`	`status.set_running_primitive(true);`
`85`	`85`
`86`	`86`	`switch (current_primitive_.primitive_case())`
Original file line number	Diff line number	Diff line change
`@@ -84,7 +84,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,`
`84`	`84`	`kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))),`
`85`	`85`	`chip_pulse_width_(`
`86`	`86`	`std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))),`
`87`		`- primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants)`
	`87`	`+ primitive_executor_(robot_constants)`
`88`	`88`	`{`
`89`	`89`	`waitForNetworkUp();`
`90`	`90`
`@@ -286,8 +286,8 @@ void Thunderloop::runLoop()`
`286`	`286`	`primitive_executor_.updatePrimitive(*createStopPrimitiveProto());`
`287`	`287`	`}`
`288`	`288`
`289`		`- direct_control_ =`
`290`		`- *primitive_executor_.stepPrimitive(primitive_executor_status_);`
	`289`	`+ direct_control_ = *primitive_executor_.stepPrimitive(`
	`290`	`+ primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_));`
`291`	`291`	`}`
`292`	`292`
`293`	`293`	`thunderloop_status_.set_primitive_executor_step_time_ms(`