Module: MJScene

class MJScene : public DynamicObject
#include <MJScene.h>

Represents a dynamic object that solves multi-body dynamics through MuJoCo.

The MJScene class defines a multi-body system where bodies are connected by joints, allowing forces and torques to act at specific points or joints. The state of the system is advanced in time through integration, with forward kinematics transforming joint states into body positions and orientations.

Public Functions

MJScene(std::string xmlString, const std::vector<std::string> &files = {})

Constructs an MJScene object using an XML string.

For a detailed reference on the XML format, see: https://mujoco.readthedocs.io/en/stable/XMLreference.html

Parameters:

  • xmlString – The XML string defining the MuJoCo model.

  • files – Optional additional files for the model.

MJScene(const MJScene&) = delete
MJScene(MJScene&&) = delete
MJScene &operator=(const MJScene&) = delete
MJScene &operator=(MJScene&&) = delete
MJBody &getBody(const std::string &name)

Retrieves a body by name.

Parameters:

name – The name of the body.

Throws:

std::invalid_argument – If the body does not exist.

Returns:

A reference to the MJBody.

MJSite &getSite(const std::string &name)

Retrieves a site by name.

Parameters:

name – The name of the site.

Throws:

std::invalid_argument – If the site does not exist.

Returns:

A reference to the MJSite.

MJEquality &getEquality(const std::string &name)

Retrieves an equality constraint by name.

Parameters:

name – The name of the equality constraint.

Throws:

std::invalid_argument – If the equality does not exist.

Returns:

A reference to the MJEquality.

MJSingleActuator &getSingleActuator(const std::string &name)

Retrieves a single-input actuator by name.

See the documentation on MJSingleActuator for details.

Parameters:

name – The name of the actuator.

Returns:

A reference to the MJSingleActuator.

MJForceActuator &getForceActuator(const std::string &name)

Retrieves a force actuator by name.

See the documentation on MJForceActuator for details.

Parameters:

name – The name of the actuator.

Returns:

A reference to the MJForceActuator.

MJTorqueActuator &getTorqueActuator(const std::string &name)

Retrieves a torque actuator by name.

See the documentation on MJTorqueActuator for details.

Parameters:

name – The name of the actuator.

Returns:

A reference to the MJTorqueActuator.

MJForceTorqueActuator &getForceTorqueActuator(const std::string &name)

Retrieves a force-torque actuator by name.

See the documentation on MJForceTorqueActuator for details.

Parameters:

name – The name of the actuator.

Returns:

A reference to the MJForceTorqueActuator.

MJSingleActuator &addSingleActuator(const std::string &name, const std::string &site, const Eigen::Vector6d &gear)

Adds a single-input actuator (MJSingleActuator).

This actuator will impart a force and/or torque at the given site.

The gear argument defines how the single input of the actuator (the control value of this actuator is a single value) maps into a force and/or torque. For example:

  • For gear={1,0,0,0,0,0} and ctrl=10, the actuator will impart 10 N of force along the X-axis of the site reference frame.

  • For gear={1,0,0,0,1,0} and ctrl=10, the actuator will impart 10 N of force along the X-axis of the site reference frame and 10 Nm of torque along the Y-axis of the same reference frame.

Parameters:

  • name – The name of the actuator.

  • site – The site to attach the actuator.

  • gear – The gear parameters for the actuator.

Returns:

Reference to the created MJSingleActuator.

MJSingleActuator &addSingleActuator(const std::string &name, const MJSite &site, const Eigen::Vector6d &gear)

Adds a single-input actuator (MJSingleActuator).

This actuator will impart a force and/or torque at the given site.

The gear argument defines how the single input of the actuator (the control value of this actuator is a single value) maps into a force and/or torque. For example:

  • For gear={1,0,0,0,0,0} and ctrl=10, the actuator will impart 10 N of force along the X-axis of the site reference frame.

  • For gear={1,0,0,0,1,0} and ctrl=10, the actuator will impart 10 N of force along the X-axis of the site reference frame and 10 Nm of torque along the Y-axis of the same reference frame.

Parameters:

  • name – The name of the actuator.

  • site – The site to attach the actuator.

  • gear – The gear parameters for the actuator.

Returns:

Reference to the created MJSingleActuator.

MJForceActuator &addForceActuator(const std::string &name, const std::string &site)

Adds a force actuator to the scene.

This actuator can impart an arbitrary force vector at the given site.

This actuator takes three input controls: the force magnitude along each axis of the site’s reference frame ([f_x, f_y, f_z]).

Parameters:

  • name – The name of the actuator.

  • site – The site associated with the actuator.

Returns:

A reference to the newly added MJForceActuator.

MJForceActuator &addForceActuator(const std::string &name, const MJSite &site)

Adds a force actuator to the scene.

This actuator can impart an arbitrary force vector at the given site.

This actuator takes three input controls: the force magnitude along each axis of the site’s reference frame ([f_x, f_y, f_z]).

Parameters:

  • name – The name of the actuator.

  • site – The site associated with the actuator.

Returns:

A reference to the newly added MJForceActuator.

MJTorqueActuator &addTorqueActuator(const std::string &name, const std::string &site)

Adds a torque actuator to the scene.

This actuator can impart an arbitrary torque vector at the given site.

This actuator takes three input controls: the torque magnitude along each axis of the site’s reference frame ([t_x, t_y, t_z]).

Parameters:

  • name – The name of the actuator.

  • site – The site associated with the actuator.

Returns:

A reference to the newly added MJTorqueActuator.

MJTorqueActuator &addTorqueActuator(const std::string &name, const MJSite &site)

Adds a torque actuator to the scene.

This actuator can impart an arbitrary torque vector at the given site.

This actuator takes three input controls: the torque magnitude along each axis of the site’s reference frame ([t_x, t_y, t_z]).

Parameters:

  • name – The name of the actuator.

  • site – The site associated with the actuator.

Returns:

A reference to the newly added MJTorqueActuator.

MJForceTorqueActuator &addForceTorqueActuator(const std::string &name, const std::string &site)

Adds a force-torque actuator to the scene.

This actuator can impart an arbitrary force and torque vectors at the given site.

This actuator takes six input controls: the force and torque magnitude along each axis of the site’s reference frame ([f_x, f_y, f_z, t_x, t_y, t_z]).

Parameters:

  • name – The name of the actuator.

  • site – The site associated with the actuator.

Returns:

A reference to the newly added MJForceTorqueActuator.

MJForceTorqueActuator &addForceTorqueActuator(const std::string &name, const MJSite &site)

Adds a force-torque actuator to the scene.

This actuator can impart an arbitrary force and torque vectors at the given site.

This actuator takes six input controls: the force and torque magnitude along each axis of the site’s reference frame ([f_x, f_y, f_z, t_x, t_y, t_z]).

Parameters:

  • name – The name of the actuator.

  • site – The site associated with the actuator.

Returns:

A reference to the newly added MJForceTorqueActuator.

void AddModelToDynamicsTask(SysModel *model, int32_t priority = -1)

Adds a model to the dynamics task.

The dynamics task of this dynamical object is called once per integrator sub-step (so, it will be called 4 times if using the RK4 integrator, for example). Models in the dynamics task typically compute and apply forces and torques on the system that depend on the simulation time or the state of the multi-body.

Warning

SysModel added to the dynamics task should be memoryless. That is, any output message computed should depend strictly on input messages and the current time/states. It should save values internally with the expectation of using them in further calls to UpdateState. The reason for this is that UpdateState may be called at arbitrary simulation times, and not necessarily in order or with consistent time steps.

Parameters:

  • model – The system model to add.

  • priority – The priority of the model in the task list.

void AddFwdKinematicsToDynamicsTask(int32_t priority)

Adds forward kinematics to the dynamics task.

The state of the multi-body is given by the state of its joints. However, we are usually interested in positions/velocities of sites. Forward kinematics is the process through which joint states are transformed to site position/velocities. A forward dynamics calculation is done by a special ‘forward-kinematics’ model.

The dynamics task of an MJScene comes preloaded with a ‘forward-kinematics’ model at priority MJScene::FWD_KINEMATICS_PRIORITY (100). This high tolerance means that, usually, whatever joint state is present at the start of the dynamics task, it will be transformed into site position/velocity such that subsequent models can query this information.

Advance usage of this class might update the joint state of the multi-body midway though its dynamics task. For example, this can be used to simulate joints with prescribed states (not implemented at the moment). In this case, one would have to add an additional forward-kinematics models such that this updated joint state is transformed into site position/velocity information and thus subsequent models query the correct information.

A safe practice is to always add forward-kinematics models after updating the joint state.

Parameters:

priority – The priority of the forward kinematics in the task list.

void SelfInit() override

Calls SelfInit on all system models in the dynamics task.

void Reset(uint64_t CurrentSimNanos) override

Calls Reset on all system models in the dynamics task and calls initializeDynamics.

Parameters:

CurrentSimNanos – The current simulation time in nanoseconds.

void initializeDynamics() override

Registers the dynamic states and recompiles the MuJoCo model.

void UpdateState(uint64_t callTime) override

Integrates the dynamics up to the given time and writes output messages.

Parameters:

callTime – The current simulation time.

void equationsOfMotion(double t, double timeStep) override

Computes the equations of motion for the system.

This evaluates the dynamics task for the given time, which should update the control values of actuators, the positions of constrained joints… These information is translated, through MuJoCo, into first derivatives of the joint states and the mass of bodies.

Parameters:

  • t – The current simulation time in seconds.

  • timeStep – The integration time step.

void preIntegration(double callTime) override

Prepares the system before actual integration.

Parameters:

callTime – The current simulation time.

void postIntegration(double callTime) override

Finalizes the system after integration.

This performs forward kinematics on the final integrated states. It may also perform an extra call to the equations of motion if the extraEoMCall flag is true. This can be useful if users want to record simulation values that are computed during the dynamics task depending on the final state.

Parameters:

callTime – The current simulation time.

inline void markAsNeedingToRecompileModel()

Marks the model as needing recompilation.

This should be called everytime one performs an action that renders the existing MuJoCo model stale, such as when adding/deleting bodies/sites/actuators…

inline bool areMujocoModelConstStale()

Checks if MuJoCo model constants are stale.

MuJoCo model constants are values that are computed once and cached on the MuJoCo model from other ‘constant’ values input by the user. In MuJoCo, the mass of bodies is a ‘constant’, thus updating it requires us to recompute the dependent ‘constants’.

Returns:

True if the model constants are stale.

inline void markMujocoModelConstAsStale()

Marks MuJoCo model constants as stale.

Should be called everytime the mass of a body is updated.

See areMujocoModelConstStale.

inline bool areKinematicsStale()

Checks if forward kinematics are stale.

Kinematics (the position/velocity of bodies and sites) are stale when the joint state of the muli-body changes (since the former depends on the latter).

If kinematics are stale, this means that the current values in the site position/velocity are not up to date with the current joint states.

Returns:

True if forward kinematics are stale.

inline void markKinematicsAsStale()

Marks forward kinematics as stale.

Should be called every time the position or velocity of a joint is updated.

void writeFwdKinematicsMessages(uint64_t CurrentSimNanos)

Writes forward kinematics output messages.

Writes the site state output messages with the current values stored in the MuJoCo data.

Warning

This does NOT update the MuJoCo data itself, so the written messages may still be stale! If you want to forward the kinematics AND write the messages, call MJFwdKinematics::fwdKinematics

Parameters:

CurrentSimNanos – The current simulation time in nanoseconds.

void saveToFile(std::string filename)

Saves the current MuJoCo model to a file.

Parameters:

filename – The name of the file to save the model.

inline mjModel *getMujocoModel()

Retrieves the MuJoCo model.

Returns:

Pointer to the MuJoCo model.

inline mjData *getMujocoData()

Retrieves the MuJoCo data.

Returns:

Pointer to the MuJoCo data.

MJQPosStateData *getQposState()

Retrieves the position state data.

Throws:

std::runtime_error – If the state has not been initialized.

Returns:

Pointer to MJQPosStateData.

StateData *getQvelState()

Retrieves the velocity state data.

Throws:

std::runtime_error – If the state has not been initialized.

Returns:

Pointer to StateData.

StateData *getActState()

Retrieves the actuation state data.

Throws:

std::runtime_error – If the state has not been initialized.

Returns:

Pointer to StateData.

void printMujocoModelDebugInfo(const std::string &path)

Prints MuJoCo model debug information to a file.

Parameters:

path – The file path for debug output.

template<typename T = std::invalid_argument>
inline void logAndThrow(const std::string &error)

Logs an error message on the BKLogger and throws an exception.

Template Parameters:

T – The type of the exception to throw.

Parameters:

error – The error message.

Public Members

bool extraEoMCall = false

Flag to run the equations of motion after integration.

If true, calls equationsOfMotion once after integration has finished, thus using the appropriate states. This won’t impact the dynamics of the object. However, it will re-compute any messages output by the dynamics task with the final, proper, state, which can be useful if users want to record messages output by the models in this task. It will also recompute the derivative of the states, which again can be useful if recording accelerations.

Message<MJSceneStateMsgPayload> stateOutMsg

Message with all the the scene’s position, velocity, and actuators states.

Public Static Functions

static MJScene fromFile(const std::string &fileName)

Creates an MJScene object from a file.

The file’s content are read, then passed to the constructor of this class.

Parameters:

fileName – The name of the file containing the MuJoCo XML.

Returns:

An MJScene object.

Public Static Attributes

static const int FWD_KINEMATICS_PRIORITY = 10000

Priority for default forward kinematics model.

Protected Functions

void updateMujocoArraysFromStates()

Updates MuJoCo structs from the Basilisk StateData objects.

void writeOutputStateMessages(uint64_t CurrentSimNanos)

Writes the values of the position, velocity, and actuators states to the stateOutMsg messages.

Parameters:

CurrentSimNanos – The current simulation time in nanoseconds.

Protected Attributes

MJSpec spec

MJSpec (MuJoCo model specification wrapper) associated with this scene.

bool mjModelConstStale = false

Flag indicating stale model constants.

bool forwardKinematicsStale = true

Flag indicating stale forward kinematics.

SysModelTask dynamicsTask

Task managing models involved in the dynamics of this scene.

std::vector<std::unique_ptr<SysModel>> ownedSysModel

System models that should be cleared on this scene destruction.

MJQPosStateData *qposState

Position state data.

StateData *qvelState

Velocity state data.

StateData *actState

Actuator state data.