Time step approach update

[Bigenlight]

Hello! @sangteak601!

I noticed that since the camera update PR (#29), the simulation can run at an unexpectedly high speed under certain circumstances. This can cause inconsistent simulation behave across different machines and isn't ideal for the predictable nature often required in ROS 2 applications.

Before the update (time step follows monitor hz (currently 60)):

Screencast.from.05-23-2025.07.21.32.PM.webm

After the update:

Screencast.from.05-23-2025.07.20.38.PM.webm

You can clearly see the high speed of the objects. (Since glfwSwapInterval(1); is set to glfwSwapInterval(0);)

To resolve this, I'm proposing a change to implement a fixed-timestep simulation loop. This approach uses the wall clock to ensure the simulation advances according to the timestep value specified in the MJCF model (mjModel->opt.timestep), providing a consistent experience for all users using the same option(timestep) in MJCF.

Just changed this part in mujoco_ros2_control_node.cpp:

  // For fixed timestep simulation
  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();

  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();

    // Perform physics steps
    while (sim_time_accumulator >= physics_dt) 
    {
      mujoco_control.update();
      sim_time_accumulator -= physics_dt; // Advance according to timestep option!
    }
    rendering->update();

    // Updating cameras at ~6 Hz based on simulation time
    // TODO(eholum): Break control and rendering into separate processes
    if (mujoco_data->time - last_cam_update >= 1.0 / 6.0)
    {
      cameras->update(mujoco_model, mujoco_data);
      last_cam_update = mujoco_data->time;
    }
  }

Test video after current PR

Screencast.from.06-24-2025.05.42.10.PM.webm

I didn't added other features or clamp for fail-safe right now. As I wanted to get your feedback on this core approach first. If you agree with this direction, I could apply other feature like adding a parameter that allows users to choose between the new time step and the previous one.

What are your thoughts on this change?

(test_diff_drive.xml was just temporary changed for speed testing, will remove after)

[sangteak601]

Hi @Bigenlight, thank you for the PR. I think your suggestion makes sense. However, I would personally like to see if there is a way to get the camera working with glfwSwapInterval(1); first.

If you continue working on completing this, it would be very helpful, as reverting glfwSwapInterval(1); probably will not work.

[Bigenlight]

Hello @sangteak601,

Thanks for the feedback! As you suggested, I've added a couple of new features.

First, I've implemented a clamping method to prevent the simulation from slowing down excessively under heavy system loads.

// 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;
}

To give users more option over the simulation behavior, I've also added parameters for configuring glfwSwapInterval() and the pacing method. The node parameters now can include enable_vsync and use_wall_clock_pacing.

    node_mujoco_ros2_control = Node(
        package='mujoco_ros2_control',
        executable='mujoco_ros2_control',
        output='screen',
        parameters=[
            robot_description,
            controller_config_file,
            {'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
        ]
    )

The default settings now are enable_vsync: False (equivalent to glfwSwapInterval(0)) and use_wall_clock_pacing: False.

You can test these changes by running ros2 launch mujoco_ros2_control_demos diff_drive.launch.py and by changing the params in it. (The sphere and cylinder will be removed after.)

What are your thoughts on these changes? I'm happy to make further adjustments!