Skip to content

Commit

Permalink
refactor: moves physics loop related functions
Browse files Browse the repository at this point in the history
  • Loading branch information
@DavidPL1
DavidPL1 committed Jul 31, 2024
1 parent 13249de commit e5d6e81
Show file tree
Hide file tree
Showing 4 changed files with 268 additions and 212 deletions.
12 changes: 12 additions & 0 deletions mujoco_ros/include/mujoco_ros/mujoco_env.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Clock> &syncCPU);

/**
* @brief physics step when sim is paused.
*/
void simPausedPhysics(mjtNum &syncSim);

/**
* @brief Handles requests from other threads (viewers).
*/
Expand Down
1 change: 1 addition & 0 deletions mujoco_ros/src/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ add_library(${PROJECT_NAME}
offscreen_camera.cpp
offscreen_rendering.cpp
callbacks.cpp
physics.cpp
)

target_include_directories(${PROJECT_NAME}
Expand Down
212 changes: 0 additions & 212 deletions mujoco_ros/src/mujoco_env.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<mujoco_ros::Viewer::Clock> 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<std::recursive_mutex> 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<std::mutex> 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<double>(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<double>(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<std::mutex> 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<std::mutex> 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<std::recursive_mutex> lock(physics_thread_mutex_);
Expand All @@ -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()) {
Expand Down
Loading

0 comments on commit e5d6e81

Please sign in to comment.