From 833b4b36fcea77cbd6a71e2dc5f788a15850e992 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Mon, 16 Jun 2025 15:00:19 +0900 Subject: [PATCH 1/8] Checking time step issue --- .../mujoco_models/test_diff_drive.xml | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml index 503eac5..5bc015a 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml @@ -26,6 +26,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 6280de3ec4234b7178d8783996e199209c328b12 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Mon, 16 Jun 2025 18:58:22 +0900 Subject: [PATCH 2/8] Add fixed sim time feature. --- .../src/mujoco_ros2_control_node.cpp | 45 ++++++++++++++----- 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index 9c20abb..ad66fb7 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -76,31 +76,52 @@ int main(int argc, const char **argv) auto rendering = mujoco_ros2_control::MujocoRendering::get_instance(); rendering->init(mujoco_model, mujoco_data); RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco rendering has been successfully initialized !"); - auto cameras = std::make_unique(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; + // run main loop, target real-time simulation + mjtNum last_cam_update_sim_time = mujoco_data->time; + + // 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(); + + // This limits how much simulation time can be processed if rendering lags + // (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((1.0 / 30.0) / physics_dt) + 1; + while (rclcpp::ok() && !rendering->is_close_flag_raised()) { - // 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) + auto current_frame_wall_time = std::chrono::steady_clock::now(); + std::chrono::duration 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) { mujoco_control.update(); + sim_time_accumulator -= physics_dt; } rendering->update(); - // Updating cameras at ~6 Hz + // Updating cameras at ~6 Hz based on simulation time // TODO(eholum): Break control and rendering into separate processes - if (simstart - last_cam_update > 1.0 / 6.0) + if (mujoco_data->time - last_cam_update_sim_time >= 1.0 / 6.0) { cameras->update(mujoco_model, mujoco_data); - last_cam_update = simstart; + last_cam_update_sim_time = mujoco_data->time; } } From 329228b38ce81d20a33f70167e4811e9e131395a Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Tue, 24 Jun 2025 17:43:06 +0900 Subject: [PATCH 3/8] Remove clamp for PR --- .../src/mujoco_ros2_control_node.cpp | 20 +++---------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index ad66fb7..c163429 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -79,18 +79,13 @@ int main(int argc, const char **argv) auto cameras = std::make_unique(node); cameras->init(mujoco_model); - // run main loop, target real-time simulation - mjtNum last_cam_update_sim_time = mujoco_data->time; + mjtNum last_cam_update = mujoco_data->time; // 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(); - // This limits how much simulation time can be processed if rendering lags - // (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((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(); @@ -99,15 +94,6 @@ int main(int argc, const char **argv) 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) { @@ -118,10 +104,10 @@ int main(int argc, const char **argv) // 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_sim_time >= 1.0 / 6.0) + if (mujoco_data->time - last_cam_update >= 1.0 / 6.0) { cameras->update(mujoco_model, mujoco_data); - last_cam_update_sim_time = mujoco_data->time; + last_cam_update = mujoco_data->time; } } From b8a0156ea2d9bd7b2a0d79f39b90a4782e93c0d0 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Mon, 7 Jul 2025 11:15:07 +0900 Subject: [PATCH 4/8] Small XML change for test --- mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml index 5bc015a..f8e4150 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml @@ -1,6 +1,6 @@ - From 04bb6cec63311c96b3ffb40a05b20ed00c03aa48 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Tue, 8 Jul 2025 13:52:44 +0900 Subject: [PATCH 5/8] Add clamping for more robust system --- .../src/mujoco_ros2_control_node.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index c163429..e841044 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/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((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 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) { From 2199156c202ff25337f1b61e3c85528f989f72e2 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Tue, 8 Jul 2025 18:08:50 +0900 Subject: [PATCH 6/8] Add glfwSwapInterval value as param --- .../mujoco_ros2_control/mujoco_rendering.hpp | 2 +- mujoco_ros2_control/src/mujoco_rendering.cpp | 4 ++-- .../src/mujoco_ros2_control_node.cpp | 14 ++++++++++---- .../launch/diff_drive.launch.py | 3 ++- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp index 8b564cd..a980692 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp +++ b/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(); diff --git a/mujoco_ros2_control/src/mujoco_rendering.cpp b/mujoco_ros2_control/src/mujoco_rendering.cpp index 9036522..c546f35 100644 --- a/mujoco_ros2_control/src/mujoco_rendering.cpp +++ b/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_); } diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index e841044..fc17a8c 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -40,6 +40,12 @@ 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("enable_vsync", false); + } bool enable_vsync = node->get_parameter("enable_vsync").as_bool(); + // load and compile model char error[1000] = "Could not load binary model"; if ( @@ -74,7 +80,7 @@ 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(node); cameras->init(mujoco_model); @@ -86,9 +92,9 @@ 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" + // 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 would mean capping catch-up to what a 30Hz loop would do. + // (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((1.0 / 30.0) / physics_dt) + 1; while (rclcpp::ok() && !rendering->is_close_flag_raised()) @@ -99,7 +105,7 @@ int main(int argc, const char **argv) sim_time_accumulator += elapsed_wall_since_last_frame.count(); - // Clamp accumulator to prevent excessive catch-up if rendering is slow + // 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 diff --git a/mujoco_ros2_control_demos/launch/diff_drive.launch.py b/mujoco_ros2_control_demos/launch/diff_drive.launch.py index 54adbaf..d0fb543 100644 --- a/mujoco_ros2_control_demos/launch/diff_drive.launch.py +++ b/mujoco_ros2_control_demos/launch/diff_drive.launch.py @@ -33,7 +33,8 @@ 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 ] ) From aead6bc072dbf31353b764c7f711d265f0bfff11 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Tue, 8 Jul 2025 19:30:54 +0900 Subject: [PATCH 7/8] Add pacing method as param --- .../src/mujoco_ros2_control_node.cpp | 108 ++++++++++++------ .../launch/diff_drive.launch.py | 3 +- 2 files changed, 74 insertions(+), 37 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index fc17a8c..abc1f59 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -44,7 +44,15 @@ int main(int argc, const char **argv) if (!node->has_parameter("enable_vsync")) { node->declare_parameter("enable_vsync", false); - } bool enable_vsync = node->get_parameter("enable_vsync").as_bool(); + } + 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("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"; @@ -87,50 +95,78 @@ int main(int argc, const char **argv) mjtNum last_cam_update = mujoco_data->time; - // 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(); - - // 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((1.0 / 30.0) / physics_dt) + 1; - - 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) { - auto current_frame_wall_time = std::chrono::steady_clock::now(); - std::chrono::duration 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; - } + 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(); - // Perform physics steps - while (sim_time_accumulator >= physics_dt) + // 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((1.0 / 30.0) / physics_dt) + 1; + + while (rclcpp::ok() && !rendering->is_close_flag_raised()) { - mujoco_control.update(); - sim_time_accumulator -= physics_dt; + auto current_frame_wall_time = std::chrono::steady_clock::now(); + std::chrono::duration 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 based on simulation time - // TODO(eholum): Break control and rendering into separate processes - if (mujoco_data->time - 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 = mujoco_data->time; + // 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; + } } } + rendering->close(); cameras->close(); diff --git a/mujoco_ros2_control_demos/launch/diff_drive.launch.py b/mujoco_ros2_control_demos/launch/diff_drive.launch.py index d0fb543..c7bc87d 100644 --- a/mujoco_ros2_control_demos/launch/diff_drive.launch.py +++ b/mujoco_ros2_control_demos/launch/diff_drive.launch.py @@ -34,7 +34,8 @@ def generate_launch_description(): 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 + {'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 ] ) From 7d8594fe5f0af0d221e1c0ed9b34482738413b44 Mon Sep 17 00:00:00 2001 From: Theo Choi Date: Tue, 8 Jul 2025 21:43:33 +0900 Subject: [PATCH 8/8] Check format --- .../src/mujoco_ros2_control_node.cpp | 16 +++++++++------- .../mujoco_models/test_diff_drive.xml | 8 ++++---- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index abc1f59..c1dfd4f 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -44,7 +44,7 @@ int main(int argc, const char **argv) if (!node->has_parameter("enable_vsync")) { node->declare_parameter("enable_vsync", false); - } + } bool enable_vsync = node->get_parameter("enable_vsync").as_bool(); // Parameter to select simulation loop pacing method. @@ -100,7 +100,8 @@ int main(int argc, const char **argv) if (use_wall_clock_pacing) { 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) + 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(); @@ -112,7 +113,8 @@ int main(int argc, const char **argv) while (rclcpp::ok() && !rendering->is_close_flag_raised()) { auto current_frame_wall_time = std::chrono::steady_clock::now(); - std::chrono::duration elapsed_wall_since_last_frame = current_frame_wall_time - last_frame_wall_time; + std::chrono::duration 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(); @@ -120,9 +122,10 @@ int main(int argc, const char **argv) // 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); + 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; } @@ -166,7 +169,6 @@ int main(int argc, const char **argv) } } - rendering->close(); cameras->close(); diff --git a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml index f8e4150..c863892 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml @@ -26,8 +26,8 @@ - - + + - - + +