ompl_pymujoco

OMPL MuJoCo Motion Planning Utilities.

This module provides a collection of utilities that integrate the MuJoCo physics engine with the OMPL (Open Motion Planning Library) to perform collision-aware motion planning in joint space.

It offers helper functions for:

  • Creating OMPL state spaces and converting between OMPL states and NumPy vectors.

  • Building MuJoCo-based state-validity checkers that detect collisions using MuJoCo’s contact data.

  • Configuring a wide selection of OMPL planners, including sampling-based, optimal, and meta-planners (e.g., BIT*, RRT#, PRM*, CForest, APS).

  • Performing complete motion-planning queries returning both raw and interpolated paths.

  • Planning motions for MuJoCo-modeled robots (or multi-robot systems) with optional clearance inflation for more conservative planning.

The core functionality revolves around mapping OMPL states to MuJoCo qpos, invoking MuJoCo forward dynamics to detect collisions, and using standard OMPL pipelines to generate feasible paths.

Dependencies

Typical Workflow

  1. Extract robot joint limits from a MuJoCo model.

  2. Build an OMPL RealVector state space.

  3. Construct a MuJoCo-based validity checker using make_mujoco_validity_fn().

  4. Set and configure an OMPL planner.

  5. Solve the motion-planning problem with plan_motion() or plan_robot_motion().

Notes

  • MuJoCo MjData is not thread-safe. For safety, a private copy of MjData is used internally for validity checking.

  • Some OMPL planners may not be available in all Python builds; unknown planner names result in informative errors.

  • Collision checking can consider only robot contacts or all contacts, depending on user settings.

This module is intended for research, prototyping, and teaching applications involving sampling-based planning for articulated robots in simulation.

robotblockset.mujoco.ompl_pymujoco.make_state_space(bounds: ndarray) → ompl.base.RealVectorStateSpace[source]

Create a RealVector state space with the given bounds.

Parameters:

bounds (np.ndarray) – Per-dimension lower/upper limits; shape (n, 2).

Returns:

State space with bounds applied.

Return type:

ompl.base.RealVectorStateSpace

Raises:

ValueError – If bounds is not (n, 2) or any low > high.

robotblockset.mujoco.ompl_pymujoco.vec_to_state(space: ompl.base.StateSpace, v: Sequence[float]) → ompl.base.State[source]

Convert a vector of coordinates to an OMPL state.

Parameters:

  • space (ob.StateSpace) – Target state space (e.g., RealVectorStateSpace).

  • v (Sequence[float]) – Coordinates; length must equal space.getDimension().

Returns:

Newly created state with values from v.

Return type:

ompl.base.State

Raises:

ValueError – If len(v) does not match the space dimension.

robotblockset.mujoco.ompl_pymujoco.state_to_vec(s: ompl.base.State, n: int) → ndarray[source]

Convert an OMPL state to a NumPy vector.

Parameters:

  • s (ob.State) – State to convert.

  • n (int) – Dimension of the state (e.g., space.getDimension()).

Returns:

State coordinates as a NumPy array.

Return type:

(n,) ndarray of float

robotblockset.mujoco.ompl_pymujoco.make_mujoco_validity_fn(model: mujoco.MjModel, data: mujoco.MjData, robot_geoms_ids: Iterable[int], qaddr: Sequence[int], *, only_robot_contacts: bool = True) → Callable[[ompl.base.State], bool][source]

Build an OMPL state-validity function that uses MuJoCo contacts.

The returned callable maps an OMPL state (joint vector) to True iff the MuJoCo scene has no contacts. By default, only contacts where either geom belongs to the robot are considered (set only_robot_contacts=False to consider all contacts).

Parameters:

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

  • data (mujoco.MjData) – A reference data object providing the template qpos (start pose). This object is not mutated.

  • robot_geoms_ids (Iterable[int]) – IDs of geoms that belong to the robot (used to filter contacts).

  • qaddr (Sequence[int]) – Indices into qpos for the planned joints (order must match the OMPL RealVector state).

  • only_robot_contacts (bool, optional) – If True (default), only contacts where geom1 or geom2 is in robot_geom_ids are considered. If False, any contact invalidates the state.

Returns:

A function that returns True for valid (collision-free) states.

Return type:

Callable[[ompl.base.State], bool]

Notes

  • A private MjData is created for thread-safety; MuJoCo data is not thread-safe to share across calls.

  • contact.dist is positive when separated, 0 at touching, and negative when penetrating.

robotblockset.mujoco.ompl_pymujoco.set_optimizing_planner(ss: ompl.geometric.SimpleSetup, name: str, *, objective: ompl.base.OptimizationObjective | None = None) → ompl.base.Planner[source]

Attach an optimizing OMPL planner to a SimpleSetup by name.

This convenience maps common planner names (e.g., "BIT*" or "RRT#") to their Python classes in ompl.geometric and sets a path-length optimization objective by default.

Parameters:

  • ss (og.SimpleSetup) – Problem container with a configured state space and validity checker.

  • name (str) – Planner name (case-insensitive). Supported keys include: "PRM*" (PRMstar), "LazyPRM*" (LazyPRMstar), "RRT*" (RRTstar), "RRT#" (RRTsharp), "RRTX" (RRTXstatic), "Informed RRT*" (InformedRRTstar), "BIT*" (BITstar), "ABIT*" (ABITstar), "AIT*" (AITstar), "EIT*" (EITstar), "LBTRRT" (LBTRRT), "SST" (SST), "T-RRT" (TRRT), "SPARS" (SPARS), "SPARS2" (SPARStwo), "FMT*" (FMT), "ST-RRT*" (STRRTstar), meta-planners: "CForest" (CForest), "APS" / "AnytimePathShortening".

  • objective (ob.OptimizationObjective, optional) – Custom objective (defaults to PathLengthOptimizationObjective). Meta-planners and *-star variants expect objectives that obey the triangle inequality (e.g., path length).

Returns:

The instantiated planner attached to ss via ss.setPlanner(...).

Return type:

ompl.base.Planner

Raises:

ValueError – If the planner name is unknown or not available in the current build.

Notes

  • Meta-planners require adding sub-planners:

    • CForest runs several optimal planners in parallel; add with planner.addPlanner(...).

    • AnytimePathShortening (APS) wraps one or more planners and repeatedly hybridizes/shortcuts; add with planner.addPlanner(...).

  • Some classes such as ABITstar, AITstar, EITstar, and RRTXstatic may not be present in older OMPL Python wheels.

robotblockset.mujoco.ompl_pymujoco.plan_motion(q_start: ndarray, q_goal: ndarray, bounds: ndarray, validity_fn: Callable[[ompl.base.State], bool], *, algorithm: str | None = 'RRTConnect', objective: ompl.base.OptimizationObjective | None = None, max_planning_time: float = 5.0, edge_resolution: float = 0.01) → Tuple[ndarray | None, ndarray | None][source]

Plan a collision-free joint-space path with OMPL and return both the raw and interpolated waypoint arrays.

Parameters:

  • q_start (JointConfigurationType) – Start joint vector (length n). Must lie within bounds.

  • q_goal (JointConfigurationType) – Goal joint vector (length n). Must lie within bounds.

  • bounds (np.ndarray) – Joint limits for a RealVector state space; each row is [low, high].

  • validity_fn (Callable[[ob.State], bool]) – State-validity checker that returns True iff the state is collision-free. (Typically calls MuJoCo to check contacts.)

  • algorithm (str, optional) – Planner to use. If None, no planner is set (use OMPL defaults). Supported optimization planners include: "PRM*" (PRMstar), "LazyPRM*" (LazyPRMstar), "RRT*" (RRTstar), "RRT#" (RRTsharp), "RRTX" (RRTXstatic), "Informed RRT*" (InformedRRTstar), "BIT*" (BITstar), "ABIT*" (ABITstar), "AIT*" (AITstar), "EIT*" (EITstar), "LBTRRT" (LBTRRT), "SST" (SST), "T-RRT" (TRRT), "SPARS" (SPARS), "SPARS2" (SPARStwo), "FMT*" (FMT), and "ST-RRT*" (STRRTstar). For the optimization planners, a path-length objective in joint space is used.

  • objective (ob.OptimizationObjective, optional) – Custom objective (defaults to PathLengthOptimizationObjective). Meta-planners and star-variant planners expect objectives that obey the triangle inequality (e.g., path length).

  • max_planning_time (float, optional) – Initial solve time budget in seconds. The routine may call solve again to continue improving the solution.

  • edge_resolution (float, optional) – Fraction of state space extent used for discrete validity checking along edges (smaller is safer but slower), e.g. 0.01 = 1% of extent.

Returns:

  • path_int ((m, n) ndarray or None) – Interpolated path waypoints after simplification (m points, including start and goal). None if planning failed.

  • waypoints ((k, n) ndarray or None) – Raw solution path waypoints before interpolation (k points). None if planning failed.

Raises:

ValueError – If an unsupported algorithm is requested.

Notes

  • This builds a RealVector state space from bounds.

  • Post-processing uses SimpleSetup.simplifySolution() for portability across OMPL Python builds, then upsamples to 100 points via path.interpolate(100).

robotblockset.mujoco.ompl_pymujoco.plan_robot_motion(robot: robot, q_start: JointConfigurationType, q_goal: JointConfigurationType, algorithm: str | None = 'RRTConnect', max_planning_time: float = 5.0, clearance: float = 0.0, only_robot_contacts: bool = True, edge_resolution: float = 0.01, validity_fn: Callable[[ob.State], bool] | None = None, objective: ob.OptimizationObjective | None = None) → Tuple[np.ndarray | None, np.ndarray | None][source]

Plan a collision-free joint-space motion for a single robot or a composite multi-robot object using MuJoCo for collision checks and OMPL for planning.

The function temporarily inflates the robot geoms’ collision margins by clearance during planning (for a safety buffer) and restores the original margins afterwards.

Parameters:

  • robot ('robot') – robot Either a single robot object exposing scene, JointNames, BaseName; or a composite with robots (each having those fields).

  • q_start (JointConfigurationType) – Start joint configuration (order must match joint_names).

  • q_goal (JointConfigurationType) – Goal joint configuration.

  • algorithm (str, optional) – {‘RRTConnect’, ‘RRTstar’, ‘InformedRRTstar’, ‘BITstar’, None}, optional Planner to use (forwarded to plan_motion). Supportedplanners include: "PRM*" (PRMstar), "LazyPRM*" (LazyPRMstar), "RRT*" (RRTstar), "RRT#" (RRTsharp), "RRTX" (RRTXstatic), "Informed RRT*" (InformedRRTstar), "BIT*" (BITstar), "ABIT*" (ABITstar), "AIT*" (AITstar), "EIT*" (EITstar), "LBTRRT" (LBTRRT), "SST" (SST), "T-RRT" (TRRT), "SPARS" (SPARS), "SPARS2" (SPARStwo), "FMT*" (FMT), "ST-RRT*" (STRRTstar), For the optimization planners, a path-length objective in joint space is used.

  • max_planning_time (float, optional) – Time budget (seconds) given to the planner.

  • clearance (float, optional) – Extra safety distance [m]; implemented here by inflating robot geoms’ MuJoCo margin during planning (default 0.0).

  • only_robot_contacts (bool, optional) – If True (default), only robot contacts are considered. If False, any contact invalidates the state.

  • edge_resolution (float, optional) – along edges (smaller = denser checks).

  • validity_fn (Callable[[ob.State], bool], optional) – Custom validity function (overrides the default MuJoCo-based checker).

  • objective (ob.OptimizationObjective, optional) – Custom objective (defaults to PathLengthOptimizationObjective).

Returns:

  • path_int ((m, n) ndarray or None) – Interpolated path from plan_motion (or None if planning failed).

  • waypoints ((k, n) ndarray or None) – Raw solution waypoints from plan_motion (or None if failed).

Raises:

ValueError – If joint dimensions do not match between inputs and model.

Functions

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

Build an OMPL state-validity function that uses MuJoCo contacts.

make_state_space(bounds)

Create a RealVector state space with the given bounds.

plan_motion(q_start, q_goal, bounds, ...[, ...])

Plan a collision-free joint-space path with OMPL and return both the raw and interpolated waypoint arrays.

plan_robot_motion(robot, q_start, q_goal[, ...])

Plan a collision-free joint-space motion for a single robot or a composite multi-robot object using MuJoCo for collision checks and OMPL for planning.

set_optimizing_planner(ss, name, *[, objective])

Attach an optimizing OMPL planner to a SimpleSetup by name.

state_to_vec(s, n)

Convert an OMPL state to a NumPy vector.

vec_to_state(space, v)

Convert a vector of coordinates to an OMPL state.