ROS 2-based container for the mujoco Simulate application. More...

#include <mujoco_simulation.hpp>

Public Types
using	ResetCallback = std::function< void(bool fill_initial_state)>
	Callback invoked when the simulation's state must be reset.

Public Member Functions
	MujocoSimulation ()=default
	Construct a new Mujoco Simulation object. This is a no-op until initialization.

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).

bool	apply_keyframe (const std::string &keyframe_name)
	Apply a keyframe to the simulation by name.

void	capture_initial_state ()
	Can be called by consumers of this class to store the current state as the "initial" state.

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.

mjModel *	model ()
	Accessor for the mujoco model.

mjData *	data ()
	Accessor for the raw mujoco simulation data.

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

void	copy_physics_model (mjModel *&destination)
	Copies `mj_model_` into the provided container in a thread safe way.

void	overwrite_physics_data (mjData *source)
	Copies the provided mjData into mj_data_ in a thread safe way.

void	copy_physics_data (mjData *&destination)
	Copies `mj_data_` into the provided container in a thread safe way.

void	apply_control_data (mjData *control_data)
	Copies control fields from `control_data` into the sim data in a thread safe way.

std::recursive_mutex &	mutex () const
	Accessor for the mutex which locks access to the data and model.

uint64_t	step_count () const
	Returns the number of steps takein by the physics simulation.

Detailed Description

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.

Member Typedef Documentation

◆ ResetCallback

using mujoco_ros2_control::MujocoSimulation::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.

Parameters

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.

Member Function Documentation

◆ apply_control_data()

void mujoco_ros2_control::MujocoSimulation::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.

◆ apply_keyframe()

bool mujoco_ros2_control::MujocoSimulation::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.

◆ capture_initial_state()

void mujoco_ros2_control::MujocoSimulation::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.

◆ copy_physics_data()

void mujoco_ros2_control::MujocoSimulation::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.

◆ copy_physics_model()

void mujoco_ros2_control::MujocoSimulation::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.

◆ data()

mjData * mujoco_ros2_control::MujocoSimulation::data ( )

inline

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.

◆ initialize()

bool mujoco_ros2_control::MujocoSimulation::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.

◆ overwrite_physics_data()

void mujoco_ros2_control::MujocoSimulation::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!

◆ reset_world_state()

void mujoco_ros2_control::MujocoSimulation::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.

Note: This method assumes sim_mutex_ is already held by the caller

◆ step_count()

uint64_t mujoco_ros2_control::MujocoSimulation::step_count ( ) const