Time step approach update

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp

@@ -40,7 +40,7 @@ class MujocoRendering
   void operator=(const MujocoRendering &) = delete;
 
   static MujocoRendering *get_instance();
-  void init(mjModel *mujoco_model, mjData *mujoco_data);
+  void init(mjModel *mujoco_model, mjData *mujoco_data, bool enable_vsync);
   bool is_close_flag_raised();
   void update();
   void close();

mujoco_ros2_control/src/mujoco_rendering.cpp

@@ -47,7 +47,7 @@ MujocoRendering::MujocoRendering()
 {
 }
 
-void MujocoRendering::init(mjModel *mujoco_model, mjData *mujoco_data)
+void MujocoRendering::init(mjModel *mujoco_model, mjData *mujoco_data, bool enable_vsync)
 {
   mj_model_ = mujoco_model;
   mj_data_ = mujoco_data;
@@ -79,7 +79,7 @@ void MujocoRendering::init(mjModel *mujoco_model, mjData *mujoco_data)
 
   // This might cause tearing, but having RViz and the renderer both open can
   // wreak havoc on the rendering process.
-  glfwSwapInterval(0);
+  glfwSwapInterval(enable_vsync);
 }
 
 bool MujocoRendering::is_close_flag_raised() { return glfwWindowShouldClose(window_); }

mujoco_ros2_control/src/mujoco_ros2_control_node.cpp

@@ -40,6 +40,20 @@ int main(int argc, const char **argv)
   RCLCPP_INFO_STREAM(node->get_logger(), "Initializing mujoco_ros2_control node...");
   auto model_path = node->get_parameter("mujoco_model_path").as_string();
 
+  // Parameter to control V-Sync (monitor synchronization). Default : false
+  if (!node->has_parameter("enable_vsync"))
+  {
+    node->declare_parameter<bool>("enable_vsync", false);
+  }
+  bool enable_vsync = node->get_parameter("enable_vsync").as_bool();
+
+  // Parameter to select simulation loop pacing method.
+  if (!node->has_parameter("use_wall_clock_pacing"))
+  {
+    node->declare_parameter<bool>("use_wall_clock_pacing", false);  // Default to original method
+  }
+  bool use_wall_clock_pacing = node->get_parameter("use_wall_clock_pacing").as_bool();
+
   // load and compile model
   char error[1000] = "Could not load binary model";
   if (
@@ -74,33 +88,84 @@ int main(int argc, const char **argv)
     mju_error("Could not initialize GLFW");
   }
   auto rendering = mujoco_ros2_control::MujocoRendering::get_instance();
-  rendering->init(mujoco_model, mujoco_data);
+  rendering->init(mujoco_model, mujoco_data, enable_vsync);
   RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco rendering has been successfully initialized !");
-
   auto cameras = std::make_unique<mujoco_ros2_control::MujocoCameras>(node);
   cameras->init(mujoco_model);
 
-  // run main loop, target real-time simulation and 60 fps rendering with cameras around 6 hz
   mjtNum last_cam_update = mujoco_data->time;
-  while (rclcpp::ok() && !rendering->is_close_flag_raised())
+
+  // Wall-clock pacing method
+  // This method uses the wall clock time to pace the simulation loop, allowing for more consistent
+  if (use_wall_clock_pacing)
   {
-    // advance interactive simulation for 1/60 sec
-    //  Assuming MuJoCo can simulate faster than real-time, which it usually can,
-    //  this loop will finish on time for the next frame to be rendered at 60 fps.
-    //  Otherwise add a cpu timer and exit this loop when it is time to render.
-    mjtNum simstart = mujoco_data->time;
-    while (mujoco_data->time - simstart < 1.0 / 60.0)
+    RCLCPP_INFO(node->get_logger(), "Using wall-clock pacing for simulation loop.");
+    const double physics_dt =
+      mujoco_model->opt.timestep;  // Simulation timestep from XML (default: 0.002s)
+    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.
+    // This limits how much simulation time can be processed if rendering lags significantly.
+    // (1.0 / 30.0) / physics_dt mean capping catch up to what 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())
     {
-      mujoco_control.update();
+      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 when 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)
+      {
+        mujoco_control.update();
+        sim_time_accumulator -= physics_dt;
+      }
+      rendering->update();
+
+      // Updating cameras at ~6 Hz based on simulation time
+      if (mujoco_data->time - last_cam_update >= 1.0 / 6.0)
+      {
+        cameras->update(mujoco_model, mujoco_data);
+        last_cam_update = mujoco_data->time;
+      }
     }
-    rendering->update();
+  }
 
-    // Updating cameras at ~6 Hz
-    // TODO(eholum): Break control and rendering into separate processes
-    if (simstart - last_cam_update > 1.0 / 6.0)
+  // Default pacing method
+  else
+  {
+    // run main loop, target real-time simulation and 60 fps rendering with cameras around 6 hz
+    while (rclcpp::ok() && !rendering->is_close_flag_raised())
     {
-      cameras->update(mujoco_model, mujoco_data);
-      last_cam_update = simstart;
+      // advance interactive simulation for 1/60 sec
+      mjtNum simstart = mujoco_data->time;
+      while (mujoco_data->time - simstart < 1.0 / 60.0)
+      {
+        mujoco_control.update();
+      }
+      rendering->update();
+
+      // Updating cameras at ~6 Hz
+      if (simstart - last_cam_update > 1.0 / 6.0)
+      {
+        cameras->update(mujoco_model, mujoco_data);
+        last_cam_update = simstart;
+      }
     }
   }
 

mujoco_ros2_control_demos/launch/diff_drive.launch.py

@@ -33,7 +33,9 @@ def generate_launch_description():
         parameters=[
             robot_description,
             controller_config_file,
-            {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_diff_drive.xml')}
+            {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_diff_drive.xml')},
+            {'enable_vsync': False},  # Set to True to sync with monitor, False to run uncapped
+            {'use_wall_clock_pacing': True} # Set to True to use wall clock pacing, False for simulation time
         ]
     )
 

mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml

@@ -1,6 +1,6 @@
 <mujoco>
     <compiler angle="radian" />
-	<option gravity="0 0 -9.81">
+	<option gravity="0 0 -9.81" timestep="0.002">
 		<flag contact="enable" />
 	</option>
 	<worldbody>
@@ -26,6 +26,40 @@
                 <inertial pos="0 0 0" mass="0.005" diaginertia="0.1 0.1 0.1" />
             </body>
         </body>
+
+
+    <body name="ball1" pos="0.5 0.0 1.4">
+      <freejoint/>
+      <geom type="sphere" name="ball1" size="0.033" condim="3"
+       friction="1 .03 .003" rgba="0 0.6 0 1" contype="1" conaffinity="1" solref="0.01 1" density="600"/>
+    </body>
+    <body name="ball2" pos="0.5 0.2 1.8">
+      <freejoint/>
+      <geom type="sphere" name="ball2" size="0.033" condim="3"
+       friction="1 .03 .003" rgba="0.6 0.6 0 1" contype="1" conaffinity="1" solref="0.01 1" density="600"/>
+    </body>
+    <body name="ball3" pos="0.5 0.5 1.9">
+      <freejoint/>
+      <geom type="sphere" name="ball3" size="0.033" condim="3"
+       friction="1 .03 .003" rgba="0.0 0.2 0.7 1" contype="1" conaffinity="1" solref="0.01 1" density="600"/>
+    </body>
+    <body name="cylinder1" pos="0.51 -0.32 1.1" euler="0 1.5708 0">
+      <freejoint/>
+      <geom type="cylinder" name="cylinder1" size="0.032 0.027 0.01" condim="3"
+        rgba="0 0.7 0.7 1" contype="1" conaffinity="1" solref="0.01 1" density="500" friction="0.01 0.003 .0003"/>
+    </body>
+    <body name="cylinder2" pos="0.51 0.1 1.5" euler="0 1.5708 0">
+      <freejoint/>
+      <geom type="cylinder" name="cylinder2" size="0.032 0.027 0.01" condim="3"
+        rgba="0.7 0.2 0 1" contype="1" conaffinity="1" solref="0.01 1" density="500" friction="0.01 0.003 .0003"/>
+    </body>
+    <body name="cylinder3" pos="0.51 0.3 1.5" euler="0 1.5708 0">
+      <freejoint/>
+      <geom type="cylinder" name="cylinder3" size="0.032 0.027 0.01" condim="3"
+        rgba="0.3 0.2 0.7 1" contype="1" conaffinity="1" solref="0.01 1" density="500" friction="0.01 0.003 .0003"/>
+    </body>
+
+
 	</worldbody>
     <contact>
         <exclude body1="chassis" body2="left_wheel" />