tools_pymujoco

Utility helpers for working with Python MuJoCo models and data.

This module provides convenience functions for querying MuJoCo model structure, navigating bodies and geoms, accessing keyframes and actuator ranges, and performing environment-distance or collision checks.

robotblockset.mujoco.tools_pymujoco.save_state_to_keyframe(model: mujoco.MjModel, data: mujoco.MjData, k: int, time: float | None = None, include_act: bool = True, include_mocap: bool = True) → None[source]

Save the current simulator state into an existing MuJoCo keyframe slot.

Copies data fields into the model’s keyframe arrays at index k: simulation time, generalized position/velocity (qpos, qvel), and optionally actuator activations (act) and mocap poses (mocap_pos, mocap_quat).

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model containing the keyframe storage.

  • data (mujoco.MjData) – Current simulator data whose state will be stored.

  • k (int) – Keyframe index in [0, model.nkey) to overwrite.

  • time (float, optional) – Timestamp to store in the keyframe. If None (default), uses data.time.

  • include_act (bool, optional) – If True (default) and the model has actuators (model.na > 0), store data.act in model.key_act.

  • include_mocap (bool, optional) – If True (default) and the model has mocap bodies (model.nmocap > 0), store mocap positions and orientations in model.key_mpos and model.key_mquat.

Raises:

IndexError – If k is outside [0, model.nkey).

Notes

  • The number of keyframes (model.nkey) is fixed by the MJCF; you cannot add new keyframes at runtime—only overwrite existing ones.

  • Controls (ctrl) are not part of keyframes.

Returns:

This method overwrites the selected keyframe in place.

Return type:

None

robotblockset.mujoco.tools_pymujoco.get_keyframe_qpos(model: mujoco.MjModel, name: str) → ndarray[source]

Retrieve the qpos vector stored in a named keyframe.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model containing keyframes.

  • name (str) – Keyframe name as defined in the MJCF (<key name="...">).

Returns:

A copy of the generalized positions qpos stored in the keyframe.

Return type:

numpy.ndarray

Raises:

RuntimeError – If a keyframe with the given name is not found.

robotblockset.mujoco.tools_pymujoco.get_robot_joints_data(model: mujoco.MjModel, joint_names: Sequence[str]) → Tuple[List[int], ndarray][source]

Collect qpos addresses and joint bounds for a list of named 1-DoF joints.

Parameters:

  • model (mujoco.MjModel) – MuJoCo model.

  • joint_names (Sequence[str]) – Names of joints to include (order defines the state-vector order).

Returns:

  • qaddr (list of int) – Indices into data.qpos for each joint (length = n_joints).

  • bounds ((n_joints, 2) ndarray of float) – Lower/upper bounds per joint; unlimited hinge/slide get sane defaults.

Raises:

ValueError – If any joint name is not found in the model.

robotblockset.mujoco.tools_pymujoco.get_body_id(model: mujoco.MjModel, name_or_id: int | str) → int[source]

Resolve a body name or id to an integer id.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • name_or_id (Union[int, str]) – Body identifier as an integer id or body name.

Returns:

Body id.

Return type:

int

Raises:

ValueError – If a name is provided and not found.

robotblockset.mujoco.tools_pymujoco.get_body_children(model: mujoco.MjModel, name_or_id: int | str) → List[int][source]

Direct child bodies (ids) of a given body.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • name_or_id (Union[int, str]) – Parent body identifier as an integer id or body name.

Returns:

Child body ids (may be empty).

Return type:

list of int

robotblockset.mujoco.tools_pymujoco.get_body_descendants(model: mujoco.MjModel, name_or_id: int | str | Iterable[int | str], include_self: bool = False, dedup: bool = True) → List[int][source]

All descendant body ids for one or more roots.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • name_or_id (Union[int, str, Iterable[Union[int, str]]]) – Root body identifier or iterable of root body identifiers.

  • include_self (bool, optional) – If True, include each root body in the result.

  • dedup (bool, optional) – If True, avoid duplicates if subtrees overlap.

Returns:

Descendant body ids (order: DFS).

Return type:

list of int

robotblockset.mujoco.tools_pymujoco.get_body_names(model: mujoco.MjModel, ids: Iterable[int]) → List[str][source]

Map body IDs to names, falling back to 'bodyN' if unnamed.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • ids (Iterable[int]) – Body IDs to resolve.

Returns:

Resolved body names in the same order as ids. For unnamed bodies, the placeholder 'body{N}' is returned where N is the body ID.

Return type:

list of str

robotblockset.mujoco.tools_pymujoco.get_geoms_of_body(model: mujoco.MjModel, name_or_id: int | str | Iterable[int | str], dedup: bool = True) → List[int][source]

Geom ids directly attached to the given body/bodies (no descendants).

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • name_or_id (Union[int, str, Iterable[Union[int, str]]]) – One body identifier or an iterable of body identifiers.

  • dedup (bool, optional) – If True, remove duplicates when multiple bodies are passed.

Returns:

Geom ids.

Return type:

list of int

robotblockset.mujoco.tools_pymujoco.get_geom_names(model: mujoco.MjModel, geom_ids: Iterable[SupportsInt]) → List[str][source]

Map geom IDs to names, falling back to 'geomN' if unnamed.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • geom_ids (Iterable[SupportsInt]) – Geom IDs to resolve.

Returns:

Resolved geom names in the same order as geom_ids. For unnamed geoms, the placeholder 'geom{N}' is returned where N is the geom ID.

Return type:

list of str

robotblockset.mujoco.tools_pymujoco.get_geom_id(model: mujoco.MjModel, name_or_id: int | str) → int[source]

Resolve a geom name or id to an integer geom ID.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • name_or_id (Union[int, str]) – Geom identifier: an existing geom ID (int) or geom name (str).

Returns:

Geom ID.

Return type:

int

Raises:

ValueError – If a geom with the given name does not exist.

robotblockset.mujoco.tools_pymujoco.get_body_of_geom(model: mujoco.MjModel, name_or_id: int | str, return_name: bool = False) → int | str[source]

Get the body that owns a given geom.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • name_or_id (Union[int, str]) – Geom id or name.

  • return_name (bool, optional) – If True, return the body name instead of id.

Returns:

Body id (default) or body name if return_name=True.

Return type:

int or str

robotblockset.mujoco.tools_pymujoco.get_bodies_of_geoms(model: mujoco.MjModel, geoms: Iterable[int | str], return_names: bool = False) → List[str | int][source]

Vectorized version: get bodies for multiple geoms.

Parameters:

  • model (mujoco.MjModel) – mujoco.MjModel

  • geoms (Iterable[Union[int, str]]) – Geom ids/names.

  • return_names (bool, optional) – If True, return body names.

Returns:

Body ids (default) or names.

Return type:

list of int or str

robotblockset.mujoco.tools_pymujoco.get_joints_under_body(model: mujoco.MjModel, parent_body: int | str) → List[str][source]

Return a list of all joint names in the subtree rooted at the given body.

Parameters:

  • model (mujoco.MjModel) – mujoco.MjModel

  • parent_body_name_or_id (int or str) – Name or id of the root body (e.g. “Panda” or 1).

Returns:

Names of all joints in that subtree (excluding unnamed joints).

Return type:

list of str

robotblockset.mujoco.tools_pymujoco.get_actuators_for_joints(model: mujoco.MjModel, robot_name: str | None = None, joints: List[str] | List[int] | None = None) → Tuple[List[str], List[int]][source]
Return actuator names and IDs for the robot defined either by:

  • robot_name (body subtree), or

  • joints (list of joint names or list of joint IDs).

Parameters:

  • model (mujoco.MjModel) – mujoco.MjModel

  • robot_name (str, optional) – Name of the root body of the robot. If provided, joints_under_body is used.

  • joints (Union[List[str], List[int]], optional) – List of joint names or joint IDs.

Returns:

  • actuator_names (list[str])

  • actuator_ids (list[int])

Raises:

ValueError if neither robot_name nor joints is provided. –

robotblockset.mujoco.tools_pymujoco.min_body_env_distance_x(model: mujoco.MjModel, data: mujoco.MjData, body_name_or_id: int | str, distmax: float | None = None, env_geom_ids: Sequence[int] | None = None) → Tuple[float, Tuple[int, int] | None, ndarray | None][source]

Minimal signed distance between all geoms of one body and a set of ‘environment’ geoms (defaults to all geoms not in this body).

Parameters:

  • model (mujoco.MjModel) – mujoco.MjModel

  • data (mujoco.MjData) – mujoco.MjData

  • body_name_or_id (Union[int, str]) – Body whose geoms we treat as ‘robot body’.

  • distmax (float, optional) – Max distance searched; defaults to 2 * model.stat.extent.

  • env_geom_ids (Sequence[int], optional) – Explicit list of environment geom ids. If None, all geoms whose body != this body are used.

Returns:

  • mindist (float) – Smallest signed distance (margin-subtracted).

  • pair ((int, int) or None) – (geom_id_body, geom_id_env) that attains the minimum, or None.

  • fromto (np.ndarray or None) – Segment [x1,y1,z1,x2,y2,z2] in world coordinates, or None.

robotblockset.mujoco.tools_pymujoco.min_robot_env_distance_x(model: mujoco.MjModel, data: mujoco.MjData, robot_roots: int | str | Iterable[int | str], distmax: float | None = None, exclude_bodies: int | str | Iterable[int | str] | None = None, env_bodies: int | str | Iterable[int | str] | None = None) → tuple[float, Dict[str, Any] | None, ndarray | None][source]

Compute the minimal signed distance between robot and environment geoms.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • data (mujoco.MjData) – MuJoCo runtime data associated with model.

  • robot_roots (Union[int, str, Iterable[Union[int, str]]]) – Root body identifier, body name, or iterable of roots defining the robot subtree. All descendant bodies are included in the robot set.

  • distmax (float, optional) – Maximum distance to search. If None, defaults to 2 * model.stat.extent.

  • exclude_bodies (Union[int, str, Iterable[Union[int, str]]], optional) – Body identifier, body name, or iterable of bodies excluded from the default environment set.

  • env_bodies (Union[int, str, Iterable[Union[int, str]]], optional) – Explicit body identifier, body name, or iterable of bodies that define the environment. When provided, this overrides the default environment construction based on exclude_bodies.

Returns:

  • mindist (float) – Smallest signed distance between any robot geom and any environment geom.

  • argmin (dict or None) – Metadata describing the minimizing pair, or None if no environment geoms are available. The dictionary contains the robot body id and name, together with the minimizing robot and environment geom ids.

  • fromto (np.ndarray or None) – World-coordinate segment [x1, y1, z1, x2, y2, z2] returned by the MuJoCo distance query, or None when no pair is found.

robotblockset.mujoco.tools_pymujoco.min_body_env_distance(model: mujoco.MjModel, data: mujoco.MjData, body_name_or_id: int | str, distmax: float | None = None, env_bodies: int | str | Iterable[int | str] | None = None, contype: int | None = None) → Tuple[float, Tuple[int, int] | None, ndarray | None][source]

Minimal signed distance between all geoms of one body and geoms belonging to a set of environment bodies.

Parameters:

  • model (mujoco.MjModel) – mujoco.MjModel

  • data (mujoco.MjData) – mujoco.MjData

  • body_name_or_id (Union[int, str]) – The body whose geoms form the ‘robot side’ of the distance query.

  • distmax (float, optional) – Max distance to search; defaults to 2 * model.stat.extent.

  • env_bodies (Union[int, str, Iterable[Union[int, str]]], optional) – Bodies that define the environment. If None, all bodies except this body are treated as environment.

  • contype (int, optional) – If None, all geom pairs are considered (pure geometric distance). If an int bitmask, only geom pairs that could collide according to MuJoCo’s contact filtering (contype/conaffinity) and that involve this bit are considered.

Returns:

  • mindist (float) – Smallest signed distance (margin-subtracted).

  • pair ((int, int) or None) – (geom_id_body, geom_id_env) that attains the minimum, or None.

  • fromto (np.ndarray or None) – Segment [x1,y1,z1,x2,y2,z2] in world coordinates, or None.

robotblockset.mujoco.tools_pymujoco.min_robot_env_distance(model: mujoco.MjModel, data: mujoco.MjData, robot_roots: int | str | Iterable[int | str], distmax: float | None = None, exclude_bodies: int | str | Iterable[int | str] | None = None, env_bodies: int | str | Iterable[int | str] | None = None, contype: int | None = None) → Tuple[float, Dict[str, Any] | None, ndarray | None][source]

Compute the minimal signed distance between robot and environment geoms.

Parameters:

  • model (mujoco.MjModel) – Loaded MuJoCo model.

  • data (mujoco.MjData) – MuJoCo runtime data associated with model.

  • robot_roots (Union[int, str, Iterable[Union[int, str]]]) – Root body identifier, body name, or iterable of roots defining the robot subtree. All descendant bodies are included in the robot set.

  • distmax (float, optional) – Maximum distance to search. If None, defaults to 2 * model.stat.extent.

  • exclude_bodies (Union[int, str, Iterable[Union[int, str]]], optional) – Body identifier, body name, or iterable of bodies excluded from the default environment set.

  • env_bodies (Union[int, str, Iterable[Union[int, str]]], optional) – Explicit body identifier, body name, or iterable of bodies that define the environment. When provided, this overrides the default environment construction based on exclude_bodies.

  • contype (int, optional) – Optional contact-type bitmask used to restrict the tested geom pairs to those that could collide according to MuJoCo contact filtering.

Returns:

  • mindist (float) – Smallest signed distance between any robot geom and any environment geom.

  • argmin (dict or None) – Metadata describing the minimizing pair, or None if no valid pair is found. The dictionary contains the robot body id and name, together with the minimizing robot and environment geom ids.

  • fromto (np.ndarray or None) – Closest-point segment [x1, y1, z1, x2, y2, z2] in world coordinates, or None if no valid pair is found.

Notes

Environment bodies are selected as follows:

  • If env_bodies is provided, only those bodies are treated as the environment.

  • Otherwise, the environment consists of all bodies except the robot subtree and any bodies listed in exclude_bodies.

If contype is provided, only geom pairs compatible with MuJoCo contype/conaffinity filtering are considered.

robotblockset.mujoco.tools_pymujoco.check_collisions(q: Sequence[float], model: mujoco.MjModel, data: mujoco.MjData, robot_geoms_ids: Iterable[int], qaddr: Sequence[int], clearance: float = 0.0) → bool[source]

Check whether a configuration produces any contact closer than clearance involving the specified robot geoms.

Parameters:

  • q (Sequence[float]) – Joint vector; order must match qaddr.

  • model (mujoco.MjModel) – mujoco.MjModel

  • data (mujoco.MjData) – Provides a qpos template; not mutated.

  • robot_geoms_ids (Iterable[int]) – Geom ids that belong to the robot.

  • qaddr (Sequence[int]) – Indices in qpos for planned joints.

  • clearance (float, optional) – Any contact with dist < clearance counts as a collision.

Returns:

True if in collision (or violating clearance), False otherwise.

Return type:

bool

robotblockset.mujoco.tools_pymujoco.check_path_for_collisions(path: ndarray, robot: Any, clearance: float = 0.0) → ndarray | bool[source]

Check a single configuration or an array of configurations for collisions.

Parameters:

  • path (np.ndarray) – One q vector or a stack of q vectors.

  • robot (Any) – Robot handle (single or composite with .robots).

  • clearance (float, optional) – Distance threshold.

Returns:

Collision flags.

Return type:

bool or (m,) ndarray of bool

robotblockset.mujoco.tools_pymujoco.make_free_camera(model: mujoco.MjModel, azimuth: float = 140.0, elevation: float = -20.0, distance: float | None = None, lookat: Sequence[float] | None = None, fovy: float | None = None) → mujoco.MjvCamera[source]

Create a configured FREE camera for use with mujoco.Renderer.update_scene.

Parameters:

  • model (mujoco.MjModel) – mujoco.MjModel

  • azimuth (float, optional) – Yaw in degrees.

  • elevation (float, optional) – Pitch in degrees.

  • distance (float, optional) – Camera distance; defaults to 1.5 * model.stat.extent.

  • lookat (Sequence[float], optional) – World-space point the camera looks at; defaults to model.stat.center.

  • fovy (float, optional) – Vertical field-of-view (degrees). If given, sets model.vis.global.fovy.

Returns:

Configured camera object.

Return type:

mujoco.MjvCamera

robotblockset.mujoco.tools_pymujoco.get_actuator_range(model: mujoco.MjModel, actuator_name: int | str) → ndarray[source]

Retrieves the control range of a specific actuator in the MuJoCo model.

The control range defines the minimum and maximum values that can be applied to the actuator during the simulation. This function supports retrieving the control range using either the actuator’s index or its name.

Parameters:

  • model (mujoco.MjModel) – The MuJoCo model object from which to retrieve the actuator’s control range.

  • actuator_name (Union[int, str]) – The identifier of the actuator for which the control range is to be retrieved. This can be either the actuator’s index (int) or name (str).

Returns:

An array containing the control range [min, max] for the specified actuator.

Return type:

np.ndarray

robotblockset.mujoco.tools_pymujoco.print_body_tree_model(model: mujoco.MjModel, parent: int | str, level: int = 0) → None[source]

Print the body tree starting from the given parent body

Parameters:

  • model (mujoco.MjModel) – Compiled MuJoCo model.

  • parent (Union[int, str]) – Body id or body name from which to start printing the body tree.

  • level (int, optional) – Current level in the body tree (used for indentation).

robotblockset.mujoco.tools_pymujoco.print_body_tree_from_xml(xml_path: str, root_body_name: str = 'Robot') → None[source]

Print the body tree starting from the given root body name in an MJCF XML file.

Parameters:

  • xml_path (str) – Path to the MJCF XML file.

  • root_body_name (str, optional) – Name of the root body from which to start printing.

Return type:

None

Functions

check_collisions(q, model, data, ...[, ...])

Check whether a configuration produces any contact closer than clearance involving the specified robot geoms.

check_path_for_collisions(path, robot[, ...])

Check a single configuration or an array of configurations for collisions.

get_actuator_range(model, actuator_name)

Retrieves the control range of a specific actuator in the MuJoCo model.

get_actuators_for_joints(model[, ...])

Return actuator names and IDs for the robot defined either by:

get_bodies_of_geoms(model, geoms[, return_names])

Vectorized version: get bodies for multiple geoms.

get_body_children(model, name_or_id)

Direct child bodies (ids) of a given body.

get_body_descendants(model, name_or_id[, ...])

All descendant body ids for one or more roots.

get_body_id(model, name_or_id)

Resolve a body name or id to an integer id.

get_body_names(model, ids)

Map body IDs to names, falling back to 'bodyN' if unnamed.

get_body_of_geom(model, name_or_id[, ...])

Get the body that owns a given geom.

get_geom_id(model, name_or_id)

Resolve a geom name or id to an integer geom ID.

get_geom_names(model, geom_ids)

Map geom IDs to names, falling back to 'geomN' if unnamed.

get_geoms_of_body(model, name_or_id[, dedup])

Geom ids directly attached to the given body/bodies (no descendants).

get_joints_under_body(model, parent_body)

Return a list of all joint names in the subtree rooted at the given body.

get_keyframe_qpos(model, name)

Retrieve the qpos vector stored in a named keyframe.

get_robot_joints_data(model, joint_names)

Collect qpos addresses and joint bounds for a list of named 1-DoF joints.

make_free_camera(model[, azimuth, ...])

Create a configured FREE camera for use with mujoco.Renderer.update_scene.

min_body_env_distance(model, data, ...[, ...])

Minimal signed distance between all geoms of one body and geoms belonging to a set of environment bodies.

min_body_env_distance_x(model, data, ...[, ...])

Minimal signed distance between all geoms of one body and a set of 'environment' geoms (defaults to all geoms not in this body).

min_robot_env_distance(model, data, robot_roots)

Compute the minimal signed distance between robot and environment geoms.

min_robot_env_distance_x(model, data, ...[, ...])

Compute the minimal signed distance between robot and environment geoms.

print_body_tree_from_xml(xml_path[, ...])

Print the body tree starting from the given root body name in an MJCF XML file.

print_body_tree_model(model, parent[, level])

Print the body tree starting from the given parent body

save_state_to_keyframe(model, data, k[, ...])

Save the current simulator state into an existing MuJoCo keyframe slot.