MJQPosStateData

class MJQPosStateData : public StateData

#include <MJQPosStateData.h>

Holds and integrates the entire MuJoCo position vector (qpos).

A MuJoCo scene integrates this single bulk position state (plus one bulk velocity, one mass, and, when present, one actuator state) regardless of how many bodies or joints it contains. Keeping the state count constant keeps the Runge-Kutta integrator’s per-stage bookkeeping (ExtendedStateVector) constant too, independent of model size.

Because qpos mixes Euclidean coordinates (slide joints, translation) with unit quaternions (free/ball joint orientation), this class integrates it with MuJoCo’s own mj_integratePos, which advances quaternion blocks on the SO(3) manifold and Euclidean blocks linearly.

Optionally (highOrderIntegration) the quaternion blocks are instead advanced as a four-component rate qdot = 0.5 * q (x) (0, omega) so the attitude inherits the full order of the Runge-Kutta method (see MJScene::highOrderAttitudeIntegration). In that mode the state derivative for a quaternion block is stored as the 4-vector qdot (filled by MJScene); every other block still stores the velocity it integrates.

Public Functions

inline MJQPosStateData(std::string inName, const Eigen::MatrixXd &newState)

Constructs an MJQPosStateData object.

Parameters:

• inName – The name of the state.

• newState – The initial state matrix.

virtual std::unique_ptr<StateData> clone() const override

Creates a clone of the current state data object.

Returns:

A unique pointer to the cloned StateData object.

void configure(mjModel *mujocoModel)

Configures the state data with the given MuJoCo model.

Sizes the state (nq) and derivative (nv), and records the location of every orientation quaternion so the high-order integration mode can find them.

Parameters:

mujocoModel – Pointer to the MuJoCo model.

void setDerivative(const Eigen::MatrixXd &newDeriv) override

Sets the derivative of the position state.

In the default mode the derivative is simply qvel (length nv), which mj_integratePos consumes.

In the high-order mode the incoming qvel is expanded into a qpos-space rate (length nq): Euclidean degrees of freedom keep their velocity while each orientation quaternion is replaced by its four-component rate qdot = 0.5 * q (x) (0, omega). When the Runge-Kutta driver writes back an already stage-combined qpos-space rate (length nq), it is stored verbatim.

void propagateState(double h, std::vector<double> pseudoStep = {}) override

Propagates the position over a time step.

In the default mode this defers to mj_integratePos. In the high-order mode, every entry is advanced linearly and each quaternion block is renormalized so the attitude inherits the integrator’s full order.

Parameters:

• h – The time step for propagation.

• pseudoStep – For states driven by stochastic dynamics, the random pseudotimestep (one per noise source).

inline const std::vector<QuaternionBlock> &getQuaternionBlocks() const

Returns the quaternion blocks discovered at configure.

Public Members

bool highOrderIntegration = false

Whether to integrate quaternion blocks at the full RK order.

See MJScene::highOrderAttitudeIntegration.

Protected Attributes

mjModel *mujocoModel = nullptr

Pointer to the MuJoCo model associated with the state.

std::vector<QuaternionBlock> quaternionBlocks

Orientation quaternions embedded in qpos.

std::vector<int> euclideanQpos

qpos indices of Euclidean DOFs

std::vector<int> euclideanQvel

matching qvel indices of Euclidean DOFs

struct QuaternionBlock

#include <MJQPosStateData.h>

Describes one orientation quaternion embedded in qpos.

qposAdr is the index of the quaternion’s w component in qpos (4 contiguous entries, MuJoCo (w,x,y,z) order). qvelAdr is the index of the matching body angular velocity in qvel (3 contiguous entries).

Public Members

int qposAdr

start index of the (w,x,y,z) quaternion in qpos

int qvelAdr

start index of the body angular velocity in qvel