Model

class MuJoCoModel

Convenient class to interface with MuJoCo models.

Includes numerical gradients.

A model instance is created from an XML file. The mjModel* and mjData* properties are stored internally.

General methods

const std::string &get_file() const

Return XML file used to construct model

int get_mumber_of_bodies() const

Return number of bodies in model

This includes the worldbody!

void reset_data(const VectorXd &qpos = VectorXd(0), const VectorXd &qvel = VectorXd(0), const VectorXd &u = VectorXd(0), const ListVectorXd &xfrc = {})

Set current state of MJ data object

Each cartesian force is stacked like [Fx; Fy; Fz; taux; tauy; tauz]; All forward functions will be called after setting states.

Parameters

  • qpos: Generalized positions

  • qvel: Generalized velocities

  • u: Actuator forces/torques

  • xfrc: Cartesian applied foces

std::vector<std::pair<double, double>> get_joint_limits() const

Get joint limits corresponding to qpos vector

Joints without limits will return (-1.0e19, 1.0e19)

Return

Vector of pairs of doubles

std::vector<std::pair<double, double>> get_torque_limits() const

Get actuator limits

Unlimited actuators have bounds (-1.0e19, 1.0e19)

Return

Vector of pairs of double

const std::vector<int> &get_quat_ids() const

Return list of quaternion ids

mjModel *get_model() const

Get pointer to MJ model object

mjData *get_data() const

Get pointer to MJ data object

int get_body_id(const std::string &name) const

Return body id

Parameters

  • name: Name of the body as defined in XML

VectorXd get_default_qpos() const

Return default value for qpos

void enable_contact(bool enable_contact = true)

Enable or disable contact dynamics

Modify MuJoCo option flag.

Dynamics functions

VectorXd get_dynamics() const

Get forward dynamics

Get acceleration jacobians

These jacobians are found through finite difference

MatrixXd get_dynamics_diff_qpos()

Get derivative of acceleration w.r.t. position

Return

Matrix, size is nv x nq.

MatrixXd get_dynamics_diff_qvel()

Get derivative of acceleration w.r.t. velocity

Return

Matrix, size is nv x nv.

MatrixXd get_dynamics_diff_u()

Get derivative of acceleration w.r.t. input force

Return

Matrix, size is nv x nu.

MatrixXd get_dynamics_diff_xfrc()

Get derivative of acceleration w.r.t. cartesian forces

Return

Matrix, size is nv x (nbodies * 6).

MatrixXd get_dynamics_diff_xfrc(int body_id, FRC_TYPE type)

Get derivative of acceleration w.r.t. a segment of cartesian forces

Return

Matrix, size is nv x 3

Parameters

  • body_id: The body_id starts at 0, which is the world body.

  • type: Either force or torque

Kinematics functions

The body id refers to the MuJoCo m->bodyid. Note that this start at 0 for the worldbody.

Parameters

Vector3d get_position(int body_id) const

Get global 3D position of body (at the body frame)

Vector4d get_orientation(int body_id) const

Get global orientation (quaternion) of body

MatrixXd get_geometric_jacobian_pos(int body_id) const

Get geometric jacobian of body for position

This is the velocity jacobian:

\[ \dot{p} = J_v * qvel \]

MatrixXd get_geometric_jacobian_rot(int body_id) const

Get geometric jacobian of body for orientation

This is the velocity jacobian:

\[ \dot{p} = J_v * qvel \]

Meaning it should be multiplied with angular velocity, not quaternion rates!

MatrixXd get_position_diff_qpos(int body_id)

Get derivative of body position to qpos using finite difference

This is the position jacobian:

\[ \dot{p} = J_q * \dot{qpos} \]

Set body pose through inverse kinematics

Solver works iteratively by setting end-effector velocities. This method modifies d->qpos and d->qvel directly.

Return

True of acceptable solution was found within max. iterations

bool inverse_kinematics(int base_id, const Vector3d &base_pos, int ee_id, const Vector3d &ee_pos)

IK for a single base and end-effector position.

Parameters

  • base_id: Body id of the base

  • base_pos: Desired position of the base

  • ee_id: Body id of the end-effector

  • ee_pos: Desired position of the end-effector

bool inverse_kinematics(const ListVector3d &refs_pos, const ListVector4d &refs_quat = {}, const IKSettings &settings = IKSettings())

IK for a list of bodies, for positions and quaternions.

Each reference is a pair of the body id and the desired pose.

Parameters

  • refs_pos: Body ids and their desired location

  • refs_quat: Body ids and their desired orientation (quaternions)

  • settings: IK settings

Actuation functions

VectorXd get_actuation_torque() const

Get torque from inputs in generalized coordinates

This is different from the actuation torque d->u itself.

Return

Generalized actuation torque, size nv x 1

MatrixXd get_actuation_jacobian()

Get jacobian of input torques to generalized torques

Jacobian is built numerically. It should be constant.

Return

Jacobian, size nv x nu

Modelling functions

MatrixXd get_heightfield(int id, bool shape_only = false)

Return map of heightfield

Return

Matrix of map (absolute values)

Parameters

  • id: Hfield id

  • shape_only: Do not include data

Set height data of an existing heightfield

The base_z value is the padding between the bottom of the map and the bottom of the geom. It will protrude negatively, such that z = 0 will be global z = 0 too.

Return

bool True on no errors

Parameters

  • map: Matrix of height data

  • radius_x: Half of the x-length

  • radius_y: Half of the y-length

  • base_z: Thickness of the material below z=0

bool set_heightfield_normalized(int id, const MatrixXd &map, double max_elevation = 0.0, double radius_x = 0.0, double radius_y = 0.0, double base_z = 0.0)

Set normalized height data of an existing heightfield.

Map data must be scaled to [0 1]!

Parameters

  • id: ID of the heightfield

  • map:

  • max_elevation: Max. height of hfield

  • radius_x:

  • radius_y:

  • base_z:

bool set_heightfield(int id, MatrixXd map, double radius_x = 0.0, double radius_y = 0.0, double base_z = 0.0)

Set absolute height data of an existing heightfield

Map data is in height, [0, >] No negative values are allowed!

Parameters

  • id: ID of the heightfield

  • map:

  • radius_x:

  • radius_y:

  • base_z:

bool set_terrain(int id, MatrixXd map, double radius_x = 0.0, double radius_y = 0.0, double base_z = 0.0)

Set map to geom

Targeted geom must already have a heightfield ready. See set_heightmap()

Parameters

  • id: ID of the geom (not the heightfield!)

  • map:

  • radius_x:

  • radius_y:

  • base_z:

Public Types

enum DIFF_TYPE

Values:

enumerator DIFF_POS
enumerator DIFF_VEL
enumerator DIFF_TORQUE
enum FRC_TYPE

Values:

enumerator FRC_FORCE
enumerator FRC_TORQUE
using MatrixXd = Eigen::MatrixXd
using VectorXd = Eigen::VectorXd
using Vector3d = Eigen::Vector3d
using Vector4d = Eigen::Vector4d
using ListVector3d = std::vector<std::pair<int, Eigen::Vector3d>>
using ListVector4d = std::vector<std::pair<int, Eigen::Vector4d>>
using ListVectorXd = std::vector<std::pair<int, Eigen::VectorXd>>

Public Functions

MuJoCoModel(const std::string &model_file)

Regular constructor

MuJoCoModel(const MuJoCoModel &rhs)

Copy constructor

Current pose and data will NOT be copied! Only the model is copied and new data is initialized.

~MuJoCoModel()

Destructor

MuJoCoModel &operator=(const MuJoCoModel &rhs)

Assignment operator

this is the left-hand side, rhs is the right-hand side. If right-hand side has a different file, mjModel will be copied. mjData will always be copied.

VectorXd get_inverse_dynamics(const VectorXd &qacc_ref)

Use inverse dynamics to compute generalized force

The resulting force has dimension nv, it does not relate to actuator forces u.

Vector3d get_contact_force(int body_id)

Get sum of all contact forces on a specified body

Force is in global frame

struct IKSettings

Struct to group settings for inverse kinematics.

Used in MuJoCoModel.inverse_kinematics().

Public Functions

IKSettings()
IKSettings(int i_max, double pos_eps, double quat_eps, double pos_threshold, double quat_threshold)

Public Members

int iter_max

Iterations limit.

double pos_eps

Step size for position.

double quat_eps

Step size for quaternions.

double pos_threshold

Acceptable position error (norm2)

double quat_threshold

Acceptable quaternion error (norm2)