diff --git a/mujoco_ros/include/mujoco_ros/mujoco_env.h b/mujoco_ros/include/mujoco_ros/mujoco_env.h index 835ba49a..c12688f4 100644 --- a/mujoco_ros/include/mujoco_ros/mujoco_env.h +++ b/mujoco_ros/include/mujoco_ros/mujoco_env.h @@ -378,6 +378,18 @@ class MujocoEnv */ void physicsLoop(); + using Clock = std::chrono::steady_clock; + + /** + * @brief physics step when sim is running. + */ + void simUnpausedPhysics(mjtNum &syncSim, std::chrono::time_point &syncCPU); + + /** + * @brief physics step when sim is paused. + */ + void simPausedPhysics(mjtNum &syncSim); + /** * @brief Handles requests from other threads (viewers). */ diff --git a/mujoco_ros/src/CMakeLists.txt b/mujoco_ros/src/CMakeLists.txt index bbacab6b..9b0dabe4 100644 --- a/mujoco_ros/src/CMakeLists.txt +++ b/mujoco_ros/src/CMakeLists.txt @@ -39,6 +39,7 @@ add_library(${PROJECT_NAME} offscreen_camera.cpp offscreen_rendering.cpp callbacks.cpp + physics.cpp ) target_include_directories(${PROJECT_NAME} diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index cfeba9ad..f134b627 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -435,211 +435,6 @@ void MujocoEnv::loadPlugins() ROS_DEBUG("Done loading MujocoRosPlugins"); } -void MujocoEnv::physicsLoop() -{ - ROS_DEBUG("Physics loop started"); - is_physics_running_ = 1; - // CPU-sim syncronization point - std::chrono::time_point syncCPU; - mjtNum syncSim = 0; - - // run until asked to exit - while (ros::ok() && !settings_.exit_request.load() && num_steps_until_exit_ != 0) { - // Sleep for 1 ms or yield, to let the main thread run - // yield results in busy wait - which has better timing but kills battery life - if (settings_.run.load() && settings_.busywait) { - std::this_thread::yield(); - } else { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - // Run only if model is present - if (model_) { - if (physics_thread_mutex_.try_lock()) { - // lock the sim mutex - // std::lock_guard lock(physics_thread_mutex_); - // Running - if (settings_.run.load()) { - // record CPU time at start of iteration - const auto startCPU = mujoco_ros::Viewer::Clock::now(); - - // Elapsed CPU and simulation time since last sync - const auto elapsedCPU = startCPU - syncCPU; - double elapsedSim = data_->time - syncSim; - - // inject noise - if (ctrl_noise_std) { - // Convert rate and scale to discrete time (Ornstein-Uhlenbeck) - mjtNum rate = mju_exp(-model_->opt.timestep / mju_max(ctrl_noise_rate, mjMINVAL)); - mjtNum scale = ctrl_noise_std * mju_sqrt(1 - rate * rate); - - for (int i = 0; i < model_->nu; i++) { - // Update noise - ctrlnoise_[i] = rate * ctrlnoise_[i] + scale * mju_standardNormal(nullptr); - - // Apply noise - data_->ctrl[i] = ctrlnoise_[i]; - } - } - - // Requested slow-down factor - double slowdown = 100 / percentRealTime[settings_.real_time_index]; - - // Misalignment condition: distance from target sim time is bigger than syncsimalign - bool misaligned = mju_abs(Seconds(elapsedCPU).count() / slowdown - elapsedSim) > syncMisalign; - - // Out-of-sync (for any reason): reset sync times, step - if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 || misaligned || - settings_.speed_changed) { - // re-sync - syncCPU = startCPU; - syncSim = data_->time; - settings_.speed_changed = false; - - // run single step, let next iteration deal with timing - mj_step(model_.get(), data_.get()); - publishSimTime(data_->time); - runLastStageCbs(); - if (settings_.render_offscreen) { - // Wait until no render request is pending - while (offscreen_.request_pending.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } - std::unique_lock lock(offscreen_.render_mutex); - - for (const auto &cam_ptr : offscreen_.cams) { - if (cam_ptr->shouldRender(ros::Time(data_->time))) { - mjv_updateSceneState(model_.get(), data_.get(), &cam_ptr->vopt_, &cam_ptr->scn_state_); - runRenderCbs(&cam_ptr->scn_state_.scratch); - offscreen_.request_pending.store(true); - } - } - } - offscreen_.cond_render_request.notify_one(); - - if (num_steps_until_exit_ > 0) { - num_steps_until_exit_--; - } - } - - // In-sync: step until ahead of CPU - else { - bool measured = false; - mjtNum prevSim = data_->time; - - // If real-time is bound, run until sim steps are in sync with CPU steps, otherwise run as fast as - // possible - while ((settings_.real_time_index == 0 || - Seconds((data_->time - syncSim) * slowdown) < mujoco_ros::Viewer::Clock::now() - syncCPU) && - (mujoco_ros::Viewer::Clock::now() - startCPU < - Seconds(mujoco_ros::Viewer::render_ui_rate_lower_bound_) || - connected_viewers_.empty()) && // only break if rendering UI is actually necessary - !settings_.exit_request.load() && - num_steps_until_exit_ != 0) { - // measure slowdown before first step - if (!measured && elapsedSim) { - if (settings_.real_time_index != 0) { - sim_state_.measured_slowdown = - std::chrono::duration(elapsedCPU).count() / elapsedSim; - measured = true; - } else if (syncCPU.time_since_epoch().count() % 3 == 0) { // measure slowdown every 3rd step for - // an updated estimate - sim_state_.measured_slowdown = - std::chrono::duration(mujoco_ros::Viewer::Clock::now() - syncCPU).count() / - Seconds(data_->time - syncSim).count(); - } - } - - // Call mj_step - mj_step(model_.get(), data_.get()); - publishSimTime(data_->time); - runLastStageCbs(); - if (settings_.render_offscreen) { - // Wait until no render request is pending - while (offscreen_.request_pending.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - } - std::unique_lock lock(offscreen_.render_mutex); - - for (const auto &cam_ptr : offscreen_.cams) { - if (cam_ptr->shouldRender(ros::Time(data_->time))) { - mjv_updateSceneState(model_.get(), data_.get(), &cam_ptr->vopt_, &cam_ptr->scn_state_); - runRenderCbs(&cam_ptr->scn_state_.scratch); - offscreen_.request_pending.store(true); - } - } - } - offscreen_.cond_render_request.notify_one(); - - if (num_steps_until_exit_ > 0) { - num_steps_until_exit_--; - } - - // Break if reset - if (data_->time < prevSim) { - break; - } - } - } - } - // Paused - else { - if (settings_.env_steps_request.load() > 0) { // Action call or arrow keys used for stepping - syncSim = data_->time; - const auto startCPU = mujoco_ros::Viewer::Clock::now(); - - while (settings_.env_steps_request.load() > 0 && - (connected_viewers_.empty() || mujoco_ros::Viewer::Clock::now() - startCPU < - Seconds(mujoco_ros::Viewer::render_ui_rate_lower_bound_))) { - // Run single step - mj_step(model_.get(), data_.get()); - publishSimTime(data_->time); - runLastStageCbs(); - if (settings_.render_offscreen) { - // Wait until no render request is pending - while (offscreen_.request_pending.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } - std::unique_lock lock(offscreen_.render_mutex); - - for (const auto &cam_ptr : offscreen_.cams) { - if (cam_ptr->shouldRender(ros::Time(data_->time))) { - mjv_updateSceneState(model_.get(), data_.get(), &cam_ptr->vopt_, &cam_ptr->scn_state_); - runRenderCbs(&cam_ptr->scn_state_.scratch); - offscreen_.request_pending.store(true); - } - } - } - offscreen_.cond_render_request.notify_one(); - - settings_.env_steps_request.fetch_sub(1); // Decrement requested steps counter - // Break if reset - if (data_->time < syncSim) { - break; - } - } - } else { - // Run mj_forward, to update rendering and joint sliders - mj_forward(model_.get(), data_.get()); - publishSimTime(data_->time); - } - } - physics_thread_mutex_.unlock(); - } // Release physics_mutex - } - } - is_physics_running_ = 0; - ROS_INFO_COND(num_steps_until_exit_ == 0, "Reached requested number of steps. Exiting simulation"); - // settings_.exit_request.store(1); - if (offscreen_.render_thread_handle.joinable()) { - offscreen_.request_pending.store(true); - offscreen_.cond_render_request.notify_one(); - ROS_DEBUG("Joining offscreen render thread"); - offscreen_.render_thread_handle.join(); - } - ROS_DEBUG("Exiting physics loop"); -} - void MujocoEnv::UpdateModelFlags(const mjOption *opt) { std::unique_lock lock(physics_thread_mutex_); @@ -661,13 +456,6 @@ void MujocoEnv::startEventLoop() event_thread_handle_ = boost::thread(&MujocoEnv::eventLoop, this); } -void MujocoEnv::waitForPhysicsJoin() -{ - if (physics_thread_handle_.joinable()) { - physics_thread_handle_.join(); - } -} - void MujocoEnv::waitForEventsJoin() { if (event_thread_handle_.joinable()) { diff --git a/mujoco_ros/src/physics.cpp b/mujoco_ros/src/physics.cpp new file mode 100644 index 00000000..31f40848 --- /dev/null +++ b/mujoco_ros/src/physics.cpp @@ -0,0 +1,255 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: David P. Leins */ + +#include + +#include + +#include + +namespace mujoco_ros { +namespace mju = ::mujoco::sample_util; + +using Seconds = std::chrono::duration; +using Milliseconds = std::chrono::duration; + +void MujocoEnv::physicsLoop() +{ + ROS_DEBUG("Physics loop started"); + is_physics_running_ = 1; + // CPU-sim syncronization point + std::chrono::time_point syncCPU; + mjtNum syncSim = 0; + + // run until asked to exit + while (ros::ok() && !settings_.exit_request.load() && num_steps_until_exit_ != 0) { + // Sleep for 1 ms or yield, to let the main thread run + // yield results in busy wait - which has better timing but kills battery life + if (settings_.run.load() && settings_.busywait) { + std::this_thread::yield(); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + // Run only if model is present + if (!model_) + continue; + + // Try acquiring the sim mutex + if (!physics_thread_mutex_.try_lock()) { + // If mutex is locked, try again later + continue; + } + + // if simulation is paused + if (!settings_.run.load()) { + simPausedPhysics(syncSim); + } else { + simUnpausedPhysics(syncSim, syncCPU); + } + // unlock physics mutex + physics_thread_mutex_.unlock(); + } + is_physics_running_ = 0; + ROS_INFO_COND(num_steps_until_exit_ == 0, "Reached requested number of steps. Exiting simulation"); + // settings_.exit_request.store(1); + if (offscreen_.render_thread_handle.joinable()) { + offscreen_.request_pending.store(true); + offscreen_.cond_render_request.notify_one(); + ROS_DEBUG("Joining offscreen render thread"); + offscreen_.render_thread_handle.join(); + } + ROS_DEBUG("Exiting physics loop"); +} + +void MujocoEnv::simPausedPhysics(mjtNum &syncSim) +{ + if (settings_.env_steps_request.load() > 0) { // Action call or arrow keys used for stepping + syncSim = data_->time; + const auto startCPU = mujoco_ros::Viewer::Clock::now(); + + while (settings_.env_steps_request.load() > 0 && + (connected_viewers_.empty() || + mujoco_ros::Viewer::Clock::now() - startCPU < Seconds(mujoco_ros::Viewer::render_ui_rate_lower_bound_))) { + // Run single step + mj_step(model_.get(), data_.get()); + publishSimTime(data_->time); + runLastStageCbs(); + if (settings_.render_offscreen) { + // Wait until no render request is pending + while (offscreen_.request_pending.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } + std::unique_lock lock(offscreen_.render_mutex); + + for (const auto &cam_ptr : offscreen_.cams) { + if (cam_ptr->shouldRender(ros::Time(data_->time))) { + mjv_updateSceneState(model_.get(), data_.get(), &cam_ptr->vopt_, &cam_ptr->scn_state_); + runRenderCbs(&cam_ptr->scn_state_.scratch); + offscreen_.request_pending.store(true); + } + } + } + offscreen_.cond_render_request.notify_one(); + + settings_.env_steps_request.fetch_sub(1); // Decrement requested steps counter + // Break if reset + if (data_->time < syncSim) { + break; + } + } + } else { + // Run mj_forward, to update rendering and joint sliders + mj_forward(model_.get(), data_.get()); + publishSimTime(data_->time); + } +} + +void MujocoEnv::simUnpausedPhysics(mjtNum &syncSim, std::chrono::time_point &syncCPU) +{ + // record CPU time at start of iteration + const auto startCPU = mujoco_ros::Viewer::Clock::now(); + + // Elapsed CPU and simulation time since last sync + const auto elapsedCPU = startCPU - syncCPU; + double elapsedSim = data_->time - syncSim; + + // Requested slow-down factor + double slowdown = 100 / percentRealTime[settings_.real_time_index]; + + // Misalignment condition: distance from target sim time is bigger than syncsimalign + bool misaligned = mju_abs(Seconds(elapsedCPU).count() / slowdown - elapsedSim) > syncMisalign; + + // Out-of-sync (for any reason): reset sync times, step + if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 || misaligned || + settings_.speed_changed) { + // re-sync + syncCPU = startCPU; + syncSim = data_->time; + settings_.speed_changed = false; + + // run single step, let next iteration deal with timing + mj_step(model_.get(), data_.get()); + publishSimTime(data_->time); + runLastStageCbs(); + if (settings_.render_offscreen) { + // Wait until no render request is pending + while (offscreen_.request_pending.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } + std::unique_lock lock(offscreen_.render_mutex); + + for (const auto &cam_ptr : offscreen_.cams) { + if (cam_ptr->shouldRender(ros::Time(data_->time))) { + mjv_updateSceneState(model_.get(), data_.get(), &cam_ptr->vopt_, &cam_ptr->scn_state_); + runRenderCbs(&cam_ptr->scn_state_.scratch); + offscreen_.request_pending.store(true); + } + } + } + offscreen_.cond_render_request.notify_one(); + + if (num_steps_until_exit_ > 0) { + num_steps_until_exit_--; + } + } + + // In-sync: step until ahead of CPU + else { + bool measured = false; + mjtNum prevSim = data_->time; + + // If real-time is bound, run until sim steps are in sync with CPU steps, otherwise run as fast as + // possible + while ((settings_.real_time_index == 0 || + Seconds((data_->time - syncSim) * slowdown) < mujoco_ros::Viewer::Clock::now() - syncCPU) && + (mujoco_ros::Viewer::Clock::now() - startCPU < Seconds(mujoco_ros::Viewer::render_ui_rate_lower_bound_) || + connected_viewers_.empty()) && // only break if rendering UI is actually necessary + !settings_.exit_request.load() && + num_steps_until_exit_ != 0) { + // measure slowdown before first step + if (!measured && elapsedSim) { + if (settings_.real_time_index != 0) { + sim_state_.measured_slowdown = std::chrono::duration(elapsedCPU).count() / elapsedSim; + measured = true; + } else if (syncCPU.time_since_epoch().count() % 3 == 0) { // measure slowdown every 3rd step for + // an updated estimate + sim_state_.measured_slowdown = + std::chrono::duration(mujoco_ros::Viewer::Clock::now() - syncCPU).count() / + Seconds(data_->time - syncSim).count(); + } + } + + // Call mj_step + mj_step(model_.get(), data_.get()); + publishSimTime(data_->time); + runLastStageCbs(); + if (settings_.render_offscreen) { + // Wait until no render request is pending + while (offscreen_.request_pending.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + std::unique_lock lock(offscreen_.render_mutex); + + for (const auto &cam_ptr : offscreen_.cams) { + if (cam_ptr->shouldRender(ros::Time(data_->time))) { + mjv_updateSceneState(model_.get(), data_.get(), &cam_ptr->vopt_, &cam_ptr->scn_state_); + runRenderCbs(&cam_ptr->scn_state_.scratch); + offscreen_.request_pending.store(true); + } + } + } + offscreen_.cond_render_request.notify_one(); + + if (num_steps_until_exit_ > 0) { + num_steps_until_exit_--; + } + + // Break if reset + if (data_->time < prevSim) { + break; + } + } + } +} + +void MujocoEnv::waitForPhysicsJoin() +{ + if (physics_thread_handle_.joinable()) { + physics_thread_handle_.join(); + } +} + +} // namespace mujoco_ros