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.

Reset registers joint positions, joint velocities, mass, activation, and task-model states in the common DynParamManager. Checked buffer segments connect joint storage to MuJoCo qpos/qvel arrays. Quaternion records use an immutable update policy chosen before topology is finalized. Integrators only see that state contract.

Before a dynamics callback, the scene copies the candidate state to MuJoCo and refreshes required model quantities. Tasks then evaluate drift or diffusion. Scene mutation and recompilation are blocked while raw model/data pointers are in use. MJSpec replaces compiled model/data and then updates wrapper bindings.

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

std::vector<std::string> getBodyNames() const

Retrieves the names of all bodies in the scene.

This method does not trigger model recompilation, so it can safely be called before initialization.

Returns:

A vector of body names.

std::string getBodyParentName(const std::string &bodyName) const

Retrieves the parent body name of a given body.

Returns the string "world" if the body’s direct parent is the worldbody. Throws std::invalid_argument if no body with the given name exists. This method does not trigger model recompilation, so it can safely be called before initialization.

Parameters:

bodyName – The name of the body.

Throws:

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

Returns:

The parent body’s name, or "world" if the parent is the worldbody.

std::vector<MJGeomInfo> getGeomInfos() const

Retrieves geometry information for all geoms in the scene.

Each entry describes a geom’s shape, size, position, orientation, and color, along with which body it belongs to. This method does not trigger model recompilation.

Returns:

A vector of MJGeomInfo structs.

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 &addJointSingleActuator(const std::string &name, const std::string &joint)

Adds a single-input actuator acting on a joint (MJSingleActuator).

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

Parameters:

• name – The name of the actuator.

• joint – The joint to attach the actuator.

Returns:

Reference to the created MJSingleActuator.

MJSingleActuator &addJointSingleActuator(const std::string &name, const MJJoint &joint)

Adds a single-input actuator acting on a joint (MJSingleActuator).

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

Parameters:

• name – The name of the actuator.

• joint – The joint to attach the actuator.

Returns:

Reference to the created MJSingleActuator.

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.

Models in this task should contribute to computing f in:

\[ dx = f(t,x)\,dt \]

where x represents the state of this dynamic object.

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 AddModelToDiffusionDynamicsTask(SysModel *model, int32_t priority = -1)

Adds a model to the diffusion dynamics task.

This task and function are only relevant when the dynamics of the system are stochastic.

Similar to AddModelToDynamicsTask, except that models in this task contribute to computing g in:

\[ dx = f(t,x)\,dt + g(t,x)\,dW \]

where x is the state of the system, f represents the drift term of the dynamics (classic time derivative), and g is the diffusion term of the dynamics (which evaluates the impact of the random ‘noise’ W on the dynamics).

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 AddFwdKinematicsToDiffusionDynamicsTask(int32_t priority)

Adds forward kinematics to the diffusion dynamics task.

See AddModelToDiffusionDynamicsTask and AddFwdKinematicsToDynamicsTask.

By default, the diffusion dynamics task has no forward kinematics model, so one must be added if the diffusion term depends on the forward kinematics of the multibody.

void SelfInit() override

Calls SelfInit once on each model in either dynamics task.

void Reset(uint64_t CurrentSimNanos) override

Registers dynamics state and calls Reset once on each model in either dynamics task.

Parameters:

CurrentSimNanos – The current simulation time in nanoseconds.

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.

Computes f in:

\[ dx = f(t,x)\,dt \]

where x represents the current state of the dynamics.

Parameters:

• t – The current simulation time in seconds.

• timeStep – The integration time step.

void equationsOfMotionDiffusion(double t, double timeStep) override

Computes the diffusion of the dynamics of the system.

Only relevant for systems with stochastic dynamics.

Computes g in:

\[ dx = f(t,x)\,dt + g(t,x)\,dW \]

where x represents the current state of the dynamics.

void preIntegration(uint64_t callTimeNanos) override

Prepares the system before actual integration.

Parameters:

callTimeNanos – The current simulation time.

void postIntegration(uint64_t callTimeNanos) 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:

callTimeNanos – 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.

StateData *getActState()

Retrieves the actuation state data, or nullptr if the model has no actuator activation states or is not yet initialized.

StateData *getMassState()

Retrieves the bulk mass state data (one entry per body), or nullptr if not yet initialized.

Bodies address their own entry through their MuJoCo body id.

void printMujocoModelDebugInfo(const std::string &path)

Prints MuJoCo model debug information to a file.

Parameters:

path – The file path for debug output.

void setHighOrderAttitudeIntegration(bool enabled)

Flag to integrate free/ball-joint attitude quaternions at the full order of the chosen Runge-Kutta method.

If false (default), each attitude quaternion is advanced by a single exponential map of the integrator’s stage-averaged body rate. This is only second-order accurate on SO(3) regardless of the RK method or any adaptive tolerance (the error falls only as the task time step shrinks).

If true, attitude quaternions are integrated as a four-component rate qdot = 0.5 * q (x) (0, omega) evaluated per RK stage, so the attitude inherits the method’s full order (e.g. order 4 for RK4) and an adaptive integrator’s tolerance controls the attitude error. The result no longer bit-matches MuJoCo’s native stepper. Set before InitializeSimulation. Select the qpos attitude policy before state topology is finalized.

bool getHighOrderAttitudeIntegration() const noexcept

Return the configured qpos attitude policy mode.

Eigen::VectorXd assembleFullQpos()

Returns the full position vector, matching the layout of mjData::qpos (length nq).

Eigen::VectorXd assembleFullQvel()

Companion to assembleFullQpos, for the qvel space (length nv).

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 copyMujocoArraysFromStates(mjModel *model, mjData *data)

Copy state buffers into already-compiled MuJoCo arrays.

void synchronizeMujocoFromStates(mjModel *model, mjData *data)

Copy states and refresh mass-dependent MuJoCo constants.

void validateMujocoMassStates(const mjModel *model) const

Validate the complete bulk mass state before mutating MuJoCo.

std::array<size_t, 2> registerMujocoStates(const mjModel *model, bool highOrderAttitude)

Register MuJoCo-owned joint, mass, and activation state records.

void registerTaskModelStates()

Register states requested by models on either dynamics task.

void bindMujocoStateSegments(mjModel *model, mjData *data, bool topologyAlreadyFinalized, bool highOrderAttitude, const std::array<size_t, 2> &jointSlots)

Bind finalized joint buffer segments and seed or restore MuJoCo arrays.

void resetTaskModels(uint64_t currentSimNanos)

Reset each distinct model once and advance both task schedules.

void configureAdaptiveStateTolerances()

Configure adaptive tolerances associated with MuJoCo state semantics.

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.

void writeOutputStateMessages(uint64_t currentSimNanos, mjtSize nq, mjtSize nv, mjtSize na)

Write output using dimensions captured from the active compiled model.

void prepareOutputStateMessageStorage(mjtSize nq, mjtSize nv, mjtSize na)

Size staging output storage without mutating the published message.

void populateOutputStateMessagePayload(mjtSize nq, mjtSize nv, mjtSize na)

Fill already-sized pending output storage from finalized states.

void publishOutputStateMessage(uint64_t currentSimNanos)

Publish the already-prepared state payload.

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.

bool firstDynamicsCall = true

Flag to track if this is the first equationsOfMotion evaluation.

SysModelTask dynamicsTask

Task managing models involved in the dynamics of this scene.

SysModelTask dynamicsDiffusionTask

Task managing models involved in the diffusion stochastic dynamics of this scene.

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

System models that should be cleared on this scene destruction.

StateData *massState = nullptr

Bulk mass state (one entry per body).

StateData *actState = nullptr

Bulk actuator state (entire act).

StateBufferSegment jointQposStateSegment

Joint-bound qpos buffer span.

StateBufferSegment jointQvelStateSegment

Joint-bound qvel buffer span.

StateBufferSegment jointQposDerivativeSegment

Native qpos tangent span.

StateBufferSegment jointQvelDerivativeSegment

Joint qacc output span.

Private Functions

void requireSceneMutationAllowed(const char *operation) const

Reject scene/spec topology changes while raw MuJoCo pointers are held.

bool updateForwardKinematicsFromStates(mjModel *model, mjData *data)

Copy changed joint states into MuJoCo and refresh position/velocity quantities. Returns whether a refresh was performed.

void registerAndBindDynamicsState(mjModel *model, mjData *data, bool topologyAlreadyFinalized)

Register and bind state storage against an already compiled model.

bool modelTopologyMatches(const mjModel &candidate) const

Check a candidate compiled model against finalized state topology.

Private Members

bool highOrderAttitudeIntegration = false

Requested quaternion policy, fixed with state topology.

bool sceneMutationBlocked = false

Blocks mutation while callbacks borrow scene/model pointers.

MJSceneStateMsgPayload outputStateMessagePayload

Reusable pending payload, filled before publication.

MujocoTopology modelTopology

Accepted MuJoCo layout checked before replacement compilation commits.

Friends

friend class MJSpec

friend class MJFwdKinematics

struct MujocoTopology

Committed dimensions and joint kinds that a recompiled model must preserve. These dimensions and addresses must match the fixed state layout.

Public Functions

bool matches(const mjModel &model) const noexcept

Check dimensions and joint kinds without allocating.

Public Members

bool registered = false

True after the first complete scene Reset.

bool highOrderAttitude = false

Quaternion derivative/policy mode captured at registration.

mjtSize nq = 0

Stored joint-position scalars.

mjtSize nv = 0

Joint-velocity scalars.

mjtSize na = 0

Actuator activation scalars.

mjtSize nbody = 0

Body count, including the world body.

std::vector<int> jointTypes

Joint kinds in compiled model order.

Public Static Functions

static MujocoTopology capture(const mjModel &model, bool highOrderAttitude)

Capture model dimensions and the selected quaternion policy.