Class MujocoSimulation

Class Documentation

class MujocoSimulation

ROS 2-based container for the mujoco Simulate application.

This class wraps the MuJoCo simulation and Simulate application, while providing necessary hooks to the ros2_control system interface to enable interaction with the sim using “normal” ROS 2 constructs.

This class is responsible for mujoco model and data, along with threads for the main physics and rendering loops. It also provides several ROS interfaces for interacting with the underlying simulation - including publishing simulated time to /clock, as well as services for pausing, stepping, and resetting the simulation.

Importantly, the physics loop is intended to run at whatever speed (relative realtime) is requested by the user. It is important to not interrupt the loop with locking calls that interact with either the physics sim data, mj_data_, or the model, mj_model_. Instead, consumers of this class are provided with functions to read all sim data and provide control inputs from their own mjData containers.

The three functions relevant to interacting with the physics sim’s mjData are:

copy_physics_data(...) will lock the sim and do a full copy of the existing mj_data_ into the provided container, which can be used as the caller requires.

apply_control_data(...) will copy control inputs from the provided mjData into the physics loop. Specifically, it copies ctrl, qfrc_applied, and xfrc_applied. ctrl and qfrc_applied are copied directly into their corresponding buffers in mj_data_. However Cartesian forces from xfrc_applied competes with inputs from the Simulate’s drag function, and so is resolved separately.

overwrite_physics_data(...) will completely replace the data for the sim. Should be used with extreme caution.

Thread safety is still somewhat messy, as callers are provided with a simulation mutex that locks the model and data while the actual mujoco engine moves the sim forward. Callers need to be wary of locking that mutex external to this class, as it can have significant consequences on the simulation’s speed.

Public Types

using ResetCallback = std::function<void(bool fill_initial_state)>

Callback invoked when the simulation’s state must be reset.

Triggered by the ~/reset_world service or by a any other reset detected in the physics loop. The callback will be run with the sim mutex but after all data has been restored.

Param fill_initial_state:

When true, the caller has not already populated mj_data_->qpos/qvel/ctrl from a keyframe and the callback should restore the captured initial state. When false, a keyframe has already been applied.

Public Functions

MujocoSimulation() = default

Construct a new Mujoco Simulation object. This is a no-op until initialization.

~MujocoSimulation()
bool initialize(rclcpp::Node::SharedPtr node, const std::string &model_path, const std::string &mujoco_model_topic, double sim_speed_factor, bool headless)

Construct the Simulate application and start the UI thread (if not headless).

This initializes the Simulate app and starts the UI thread in the background (if not running headless). It also sets up required publishers and services using the provided node.

bool apply_keyframe(const std::string &keyframe_name)

Apply a keyframe to the simulation by name.

This locks the simulation mutex and attempts to apply a keyframe by name by calling mj_resetDataKeyframe.

void capture_initial_state()

Can be called by consumers of this class to store the current state as the “initial” state.

In particular, this persises qpos, qvel, and ctrl vectors from the data and writes them into our ‘initial_*’ vectors.

void set_reset_callback(ResetCallback callback)

Register a callback function to be called on reset_world_state.

void start_physics_thread()

Start the physics thread. Must be called after load_model().

void shutdown()

Stop the physics and UI threads if they are running.

inline mjModel *model()

Accessor for the mujoco model.

inline mjData *data()

Accessor for the raw mujoco simulation data.

Users should generally not interact with the physics sim data excepting during setup and other special circumstances. For “normal” processing it is recommended to use control_data or other containers populated by copy_mj_data.

void reset_world_state(bool fill_initial_state)

Reset simulation state (qpos/qvel/ctrl/sensors/forces) to the captured initial state.

Note

Caller must hold the sim mutex.

void copy_physics_model(mjModel *&destination)

Copies mj_model_ into the provided container in a thread safe way.

This locks the sim mutex and will pause the physics loop, so should be used sparingly.

void overwrite_physics_data(mjData *source)

Copies the provided mjData into mj_data_ in a thread safe way.

Note

This will completely overwrite the existing data, use with caution!

void copy_physics_data(mjData *&destination)

Copies mj_data_ into the provided container in a thread safe way.

This locks the sim mutex and will pause the physics loop, so should be used sparingly.

Note

: If the destination is null it will be created.

void apply_control_data(mjData *control_data)

Copies control fields from control_data into the sim data in a thread safe way.

Specifically, copies control_data->ctrl and control_data->qfrc_applied into mj_data_ to update the hw control inputs for the next iteration of the physics loop. control_data->xfrc_applied is copied into xfrc_plugin_desired_ to avoid conflicts from the simulate app.

inline std::recursive_mutex &mutex() const

Accessor for the mutex which locks access to the data and model.