Add clamping for more robust system

Author: Bigenlight

mujoco_ros2_control/src/mujoco_ros2_control_node.cpp

@@ -86,14 +86,28 @@ int main(int argc, const char **argv)
   double sim_time_accumulator = 0.0;
   auto last_frame_wall_time = std::chrono::steady_clock::now();
 
+  // Define a maximum number of physics steps per rendering frame to prevent "spiral of death"
+  // This limits how much simulation time can be processed if rendering lags significantly.
+  // (1.0 / 30.0) / physics_dt would mean capping catch-up to what a 30Hz loop would do.
+  const int max_physics_steps_per_render_frame = static_cast<int>((1.0 / 30.0) / physics_dt) + 1;
+
   while (rclcpp::ok() && !rendering->is_close_flag_raised())
   {
     auto current_frame_wall_time = std::chrono::steady_clock::now();
     std::chrono::duration<double> elapsed_wall_since_last_frame = current_frame_wall_time - last_frame_wall_time;
     last_frame_wall_time = current_frame_wall_time;
-
+    
     sim_time_accumulator += elapsed_wall_since_last_frame.count();
 
+    // Clamp accumulator to prevent excessive catch-up if rendering is slow
+    if (sim_time_accumulator > max_physics_steps_per_render_frame * physics_dt) {
+        RCLCPP_WARN_THROTTLE(
+            node->get_logger(), *node->get_clock(), 1000, // Log once per second if this happens
+            "Simulation is lagging; clamping accumulated time. Accumulator: %f, Max allowed: %f",
+            sim_time_accumulator, max_physics_steps_per_render_frame * physics_dt);
+        sim_time_accumulator = max_physics_steps_per_render_frame * physics_dt;
+    }
+
     // Perform physics steps
     while (sim_time_accumulator >= physics_dt)
     {