ompl_pymujoco

OMPL and MuJoCo joint-space motion-planning utilities.

This module connects MuJoCo collision checking with OMPL geometric planners. It provides helpers for creating OMPL state spaces, converting between OMPL states and NumPy vectors, configuring planners, validating paths, repairing invalid path segments, and planning collision-free robot motions directly from RobotBlockSet MuJoCo robot objects. Existing collision-free paths can also be inserted as incumbent OMPL solutions and refined further.

Notes

MuJoCo MjData is not thread-safe. JointPathPlanner keeps a private copy for state-validity checks and refreshes it when the scene model/data are updated.

Some OMPL planner classes are optional in Python builds. Unsupported or unavailable planner names raise ValueError with a descriptive message.

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

Create an OMPL real-vector state space from numeric bounds.

Parameters:

bounds (array_like, shape (n, 2)) – Lower and upper limits for each state-space dimension.

Returns:

State space with the supplied bounds.

Return type:

ompl.base.RealVectorStateSpace

Raises:

ValueError – If bounds is not two-dimensional with two columns, has no dimensions, or contains a lower bound greater than its upper bound.

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

Convert a vector of coordinates to an OMPL state.

Parameters:

  • space (ompl.base.StateSpace) – Target OMPL state space.

  • v (sequence of float) – State coordinates. The length must equal space.getDimension().

Returns:

Newly allocated state populated with 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 (ompl.base.State) – State to convert.

  • n (int) – Number of scalar coordinates to read from s.

Returns:

State coordinates.

Return type:

ndarray, shape (n,)

class robotblockset.mujoco.ompl_pymujoco.JointPathPlanner(robot: robot, q_start: VectorLike, q_goal: VectorLike, algorithm: str | None = 'RRTConnect', max_planning_time: float = 5.0, max_simplification_time: float = 0.0, clearance: float = 0.0, only_robot_contacts: bool = True, excluded_contact_geom_ids: Iterable[int] | None = None, excluded_contact_geom_pairs: Iterable[ContactGeomPairLike] | None = None, included_contact_geoms: Iterable[int] | None = None, edge_resolution: float = 0.01, interpolation_count: int = 0, validity_fn: StateValidityFn | None = None, objective: ob.OptimizationObjective | None = None)[source]

Bases: object

OMPL/MuJoCo joint-space path planner.

Attributes:

  • space (ompl.base.RealVectorStateSpace) – OMPL joint-space state space.

  • ss (ompl.geometric.SimpleSetup) – OMPL planning setup.

  • si (ompl.base.SpaceInformation) – OMPL space-information object associated with ss.

  • planner (ompl.base.Planner or None) – Currently configured planner instance.

  • qaddr (list of int) – MuJoCo qpos addresses for the planned joints.

  • bounds (ndarray, shape (n, 2)) – Joint lower and upper bounds.

  • robot_geoms (set of int) – Geom ids considered robot geoms during collision checking.

  • excluded_contact_geoms (set of int) – Geom ids ignored during built-in MuJoCo contact checking.

  • excluded_contact_geom_pairs (set of tuple of int) – Unordered geom-id pairs ignored during built-in MuJoCo contact checking.

  • included_contact_geoms (set of int) – Geom ids whose contacts always invalidate a state.

  • has_welded_objects (list of int) – Weld equality ids touching the robot body trees.

Initialize the planner and configure the OMPL problem.

Parameters:

  • robot (robot-like) – RobotBlockSet MuJoCo robot object. A composite object exposing robots is also supported; each child robot must expose scene, JointNames and BaseName.

  • q_start (array_like, shape (n,)) – Start joint configuration in the order defined by the robot joint names.

  • q_goal (array_like, shape (n,)) – Goal joint configuration in the same joint order as q_start.

  • algorithm (str or None, optional) – OMPL planner name. "RRTConnect" is used by default. Supported names include "PRM*", "LazyPRM*", "RRT*", "RRT#", "RRTX", "AORRTC", "InformedRRT*", "BIT*", "BLIT*", "ABIT*", "AIT*", "EIT*", "LBTRRT", "SST", "T-RRT", "TRRT*", "ATRRT", "SPARS", "SPARS2", "FMT*", "ST-RRT*", "CForest" and "APS". If None, OMPL’s default planner selection is used.

  • max_planning_time (float, optional) – Default time budget in seconds used by plan().

  • max_simplification_time (float, optional) – Time budget in seconds passed to SimpleSetup.simplifySolution. The default 0.0 uses OMPL’s default simplification procedure.

  • clearance (float, optional) – Collision-margin inflation applied to robot geoms while planning and validating paths.

  • only_robot_contacts (bool, optional) – If True, contacts not involving robot geoms are ignored.

  • excluded_contact_geom_ids (iterable of int, optional) – MuJoCo geom ids whose contacts are ignored by the default MuJoCo validity checker. If a contact involves any excluded geom, that contact does not invalidate the state. This argument is not applied when a custom validity_fn is supplied.

  • excluded_contact_geom_pairs (iterable of pair-like, optional) – MuJoCo geom-id pairs whose contacts are ignored by the default MuJoCo validity checker. Pair order is ignored, so (g1, g2) and (g2, g1) are equivalent. This argument is not applied when a custom validity_fn is supplied.

  • included_contact_geoms (iterable of int, optional) – MuJoCo geom ids whose contacts always invalidate the state in the default MuJoCo validity checker. If any contact involves one of these geoms, the state is invalid even if that contact would otherwise be ignored by only_robot_contacts or the exclusion filters. This argument is not applied when a custom validity_fn is supplied.

  • edge_resolution (float, optional) – OMPL validity-checking resolution as a fraction of state-space extent.

  • interpolation_count (int, optional) – Number of path states after interpolation. Values greater than 3 enable interpolation; values 0 through 3 leave the simplified solution path unchanged.

  • validity_fn (callable, optional) – Custom OMPL state-validity callback. If omitted, MuJoCo contact checking is used.

  • objective (ompl.base.OptimizationObjective, optional) – Optimization objective for planners that require one. If omitted, a path-length objective is created for optimizing planners.

Return type:

None

Raises:

ValueError – If q_start/q_goal dimensions do not match the robot joints, if max_simplification_time is negative, if an excluded geom pair does not contain exactly two ids, or if the planner name is not supported by the installed OMPL build.

__init__(robot: robot, q_start: VectorLike, q_goal: VectorLike, algorithm: str | None = 'RRTConnect', max_planning_time: float = 5.0, max_simplification_time: float = 0.0, clearance: float = 0.0, only_robot_contacts: bool = True, excluded_contact_geom_ids: Iterable[int] | None = None, excluded_contact_geom_pairs: Iterable[ContactGeomPairLike] | None = None, included_contact_geoms: Iterable[int] | None = None, edge_resolution: float = 0.01, interpolation_count: int = 0, validity_fn: StateValidityFn | None = None, objective: ob.OptimizationObjective | None = None) → None[source]

Initialize the planner and configure the OMPL problem.

Parameters:

  • robot (robot-like) – RobotBlockSet MuJoCo robot object. A composite object exposing robots is also supported; each child robot must expose scene, JointNames and BaseName.

  • q_start (array_like, shape (n,)) – Start joint configuration in the order defined by the robot joint names.

  • q_goal (array_like, shape (n,)) – Goal joint configuration in the same joint order as q_start.

  • algorithm (str or None, optional) – OMPL planner name. "RRTConnect" is used by default. Supported names include "PRM*", "LazyPRM*", "RRT*", "RRT#", "RRTX", "AORRTC", "InformedRRT*", "BIT*", "BLIT*", "ABIT*", "AIT*", "EIT*", "LBTRRT", "SST", "T-RRT", "TRRT*", "ATRRT", "SPARS", "SPARS2", "FMT*", "ST-RRT*", "CForest" and "APS". If None, OMPL’s default planner selection is used.

  • max_planning_time (float, optional) – Default time budget in seconds used by plan().

  • max_simplification_time (float, optional) – Time budget in seconds passed to SimpleSetup.simplifySolution. The default 0.0 uses OMPL’s default simplification procedure.

  • clearance (float, optional) – Collision-margin inflation applied to robot geoms while planning and validating paths.

  • only_robot_contacts (bool, optional) – If True, contacts not involving robot geoms are ignored.

  • excluded_contact_geom_ids (iterable of int, optional) – MuJoCo geom ids whose contacts are ignored by the default MuJoCo validity checker. If a contact involves any excluded geom, that contact does not invalidate the state. This argument is not applied when a custom validity_fn is supplied.

  • excluded_contact_geom_pairs (iterable of pair-like, optional) – MuJoCo geom-id pairs whose contacts are ignored by the default MuJoCo validity checker. Pair order is ignored, so (g1, g2) and (g2, g1) are equivalent. This argument is not applied when a custom validity_fn is supplied.

  • included_contact_geoms (iterable of int, optional) – MuJoCo geom ids whose contacts always invalidate the state in the default MuJoCo validity checker. If any contact involves one of these geoms, the state is invalid even if that contact would otherwise be ignored by only_robot_contacts or the exclusion filters. This argument is not applied when a custom validity_fn is supplied.

  • edge_resolution (float, optional) – OMPL validity-checking resolution as a fraction of state-space extent.

  • interpolation_count (int, optional) – Number of path states after interpolation. Values greater than 3 enable interpolation; values 0 through 3 leave the simplified solution path unchanged.

  • validity_fn (callable, optional) – Custom OMPL state-validity callback. If omitted, MuJoCo contact checking is used.

  • objective (ompl.base.OptimizationObjective, optional) – Optimization objective for planners that require one. If omitted, a path-length objective is created for optimizing planners.

Return type:

None

Raises:

ValueError – If q_start/q_goal dimensions do not match the robot joints, if max_simplification_time is negative, if an excluded geom pair does not contain exactly two ids, or if the planner name is not supported by the installed OMPL build.

update_robot_model_and_scene_data(robot: 'robot' | None = None, *, clear_solution: bool = False) → None[source]

Refresh cached robot, MuJoCo model and scene data references.

Call this after recompiling or replacing the robot scene so collision checking uses the current scene.model and scene.data. Existing OMPL planning data is preserved unless clear_solution is True. If joint bounds change, the OMPL setup is rebuilt and existing planning data cannot be preserved.

Parameters:

  • robot (robot-like, optional) – Replacement robot or composite robot object. If omitted, the existing self.robot object is queried again.

  • clear_solution (bool, optional) – If True, clear existing OMPL solution/planning data after refreshing the MuJoCo references. Defaults to False.

Return type:

None

Raises:

ValueError – If the refreshed model has a different joint dimension than the stored start or goal configuration.

set_planner(algorithm: str | None = None, *, objective: ompl.base.OptimizationObjective | None = None) → ompl.base.Planner | None[source]

Configure and attach an OMPL planner.

Parameters:

  • algorithm (str or None, optional) – Planner name. If None, OMPL’s default planner selection is left in place.

  • objective (ompl.base.OptimizationObjective, optional) – Optimization objective used by optimizing planners. If omitted, the current self.objective is reused; if that is also None, a path-length objective is created when needed.

Returns:

Configured planner instance, or None when algorithm is None.

Return type:

ompl.base.Planner or None

Raises:

ValueError – If the planner name is unknown or unavailable in the installed OMPL build.

set_start_goal(q_start: ndarray | Sequence[float], q_goal: ndarray | Sequence[float], threshold: float = 0.001) → None[source]

Set the OMPL start and goal states.

Parameters:

  • q_start (array_like, shape (n,)) – Start joint configuration.

  • q_goal (array_like, shape (n,)) – Goal joint configuration.

  • threshold (float, optional) – OMPL goal-threshold distance.

Return type:

None

Raises:

ValueError – If q_start or q_goal has the wrong dimension.

is_path_valid(path: ndarray | Sequence[float] | Sequence[Sequence[float]] | None = None, refresh_scene_state: bool = True) → bool[source]

Check whether a path is valid in the current scene.

If path is omitted, the current OMPL solution path is checked. If a waypoint array is supplied, it is converted to an OMPL PathGeometric and checked with the planner’s current validity checker and edge resolution.

Parameters:

  • path (array_like, shape (m, n), optional) – Joint-space path to validate. If omitted, the current OMPL solution path is validated.

  • refresh_scene_state (bool, optional) – If True, refresh the nominal MuJoCo qpos template from self.data.qpos before checking.

Returns:

True when all states and motions in the path are valid.

Return type:

bool

Raises:

ValueError – If path has an invalid shape or contains non-finite values.

valid_path_segments(path: ndarray | Sequence[float] | Sequence[Sequence[float]] | None = None, *, refresh_scene_state: bool = True) → list[tuple[int, int]][source]

Return maximal valid waypoint-index segments for a path.

Each returned pair is (first_index, last_index). A segment is valid when all included waypoint states are valid and all motions between consecutive included waypoints are valid.

Parameters:

  • path (array_like, shape (m, n), optional) – Joint-space path to segment. If omitted, the current OMPL solution path is used.

  • refresh_scene_state (bool, optional) – If True, refresh the nominal MuJoCo qpos template from self.data.qpos before checking.

Returns:

Maximal valid segments as inclusive waypoint-index pairs.

Return type:

list of tuple of int

Raises:

ValueError – If path has an invalid shape or contains non-finite values.

plan(q_start: ndarray | Sequence[float] | None = None, q_goal: ndarray | Sequence[float] | None = None, max_planning_time: float | None = None) → Tuple[ndarray | None, ndarray | None][source]

Solve the configured planning query.

Parameters:

  • q_start (array_like, shape (n,), optional) – Replacement start configuration. If omitted, self.q_start is used.

  • q_goal (array_like, shape (n,), optional) – Replacement goal configuration. If omitted, self.q_goal is used.

  • max_planning_time (float, optional) – Solve time budget in seconds. If omitted, self.max_planning_time is used.

Returns:

  • path (ndarray, shape (m, n), or None) – Simplified and optionally interpolated solution path.

  • waypoints (ndarray, shape (k, n), or None) – Raw solution waypoints before simplification.

Raises:

ValueError – If replacement start or goal configurations have invalid dimensions.

initialize_path(path: ndarray | Sequence[float] | Sequence[Sequence[float]], *, validate_path: bool = True, clear_existing: bool = True, set_start_goal_from_path: bool = True, threshold: float = 0.001, name: str = 'initial_path', refresh_scene_state: bool = True) → Tuple[ndarray | None, ndarray | None][source]

Initialize the current OMPL solution from path waypoints.

This method seeds the planner with an existing collision-free path so that refine_solution() or refine_with_planner() can improve it as if it had been found by plan().

Parameters:

  • path (array_like, shape (m, n)) – Joint-space waypoint path to insert as the incumbent solution.

  • validate_path (bool, optional) – If True, verify that all states and motions in path are valid before adding it to OMPL.

  • clear_existing (bool, optional) – If True, clear existing OMPL planning data before adding the supplied path.

  • set_start_goal_from_path (bool, optional) – If True, set the planning query start and goal to the first and last path waypoints.

  • threshold (float, optional) – Goal threshold used when set_start_goal_from_path is True.

  • name (str, optional) – Name attached to the inserted OMPL solution path.

  • refresh_scene_state (bool, optional) – If True and validate_path is also True, refresh the nominal MuJoCo qpos template from self.data.qpos before validating the path.

Returns:

  • initialized_path (ndarray, shape (m, n)) – Inserted path as a floating-point NumPy array.

  • initialized_waypoints (ndarray, shape (m, n)) – Copy of initialized_path for compatibility with plan().

Raises:

ValueError – If path has an invalid shape, contains non-finite values, has fewer than two waypoints, fails validation, or has endpoints that do not match the current query when set_start_goal_from_path is False.

refine_solution(additional_time: float) → Tuple[ndarray | None, ndarray | None][source]

Continue solving the current planning problem.

Repeated calls to SimpleSetup.solve let optimizing planners improve the incumbent solution without rebuilding the planning problem. Call plan() or initialize_path() first to create an incumbent.

Parameters:

additional_time (float) – Extra solve time budget in seconds. A value of 0 returns the current solution if one exists.

Returns:

  • path (ndarray, shape (m, n), or None) – Simplified and optionally interpolated solution path.

  • waypoints (ndarray, shape (k, n), or None) – Raw solution waypoints before simplification.

Raises:

ValueError – If additional_time is negative.

refine_with_planner(algorithm: str, additional_time: float, *, objective: ompl.base.OptimizationObjective | None = None) → Tuple[ndarray | None, ndarray | None][source]

Switch planners and continue from the current solution.

Call plan() first with a fast planner such as RRTConnect or seed an existing path with initialize_path(), then use this method with an optimizing planner such as InformedRRT* or BIT* to improve the path without losing the existing solution.

Parameters:

  • algorithm (str) – Planner name used for refinement.

  • additional_time (float) – Extra solve time budget in seconds.

  • objective (ompl.base.OptimizationObjective, optional) – Optimization objective for the refinement planner.

Returns:

  • path (ndarray, shape (m, n), or None) – Simplified and optionally interpolated solution path.

  • waypoints (ndarray, shape (k, n), or None) – Raw solution waypoints before simplification.

Raises:

  • ValueError – If additional_time is negative or the requested planner is unavailable.

  • RuntimeError – If no incumbent solution is available.

repair_path(path: ndarray | Sequence[float] | Sequence[Sequence[float]] | None = None, *, algorithm: str | None = None, max_planning_time: float | None = None, max_simplification_time: float | None = None, interpolation_count: int | None = None, objective: ompl.base.OptimizationObjective | None = None, refresh_scene_state: bool = True) → Tuple[ndarray | None, ndarray | None][source]

Repair a path by replanning invalid spans between valid segments.

The method finds valid path segments, replans each gap between consecutive valid segments, and concatenates the original valid segments with the newly planned bridges. If the first or last waypoint is invalid, the path cannot be repaired because there is no valid endpoint to anchor the repair.

Parameters:

  • path (array_like, shape (m, n), optional) – Path to repair. If omitted, the current OMPL solution path is used.

  • algorithm (str or None, optional) – Planner used for bridge segments. If omitted, self.algorithm is reused.

  • max_planning_time (float, optional) – Bridge-planning time budget in seconds. If omitted, self.max_planning_time is used.

  • max_simplification_time (float, optional) – Bridge simplification time budget in seconds. If omitted, self.max_simplification_time is used.

  • interpolation_count (int, optional) – Bridge interpolation count. If omitted, self.interpolation_count is used.

  • objective (ompl.base.OptimizationObjective, optional) – Optimization objective used by bridge planners.

  • refresh_scene_state (bool, optional) – If True, refresh the nominal MuJoCo qpos template from self.data.qpos before computing valid segments.

Returns:

  • repaired_path (ndarray, shape (r, n), or None) – Repaired complete path, or None if repair fails.

  • repaired_waypoints (ndarray, shape (r, n), or None) – Copy of repaired_path for compatibility with plan(). None if repair fails.

Raises:

ValueError – If path has an invalid shape, contains non-finite values, or a supplied time budget is negative. A ValueError can also be raised if a requested bridge planner is unavailable.

robotblockset.mujoco.ompl_pymujoco.plan_robot_motion(robot: robot, q_start: VectorLike, q_goal: VectorLike, algorithm: str | None = 'RRTConnect', max_planning_time: float = 5.0, max_simplification_time: float = 0.0, clearance: float = 0.0, only_robot_contacts: bool = True, excluded_contact_geom_ids: Iterable[int] | None = None, excluded_contact_geom_pairs: Iterable[ContactGeomPairLike] | None = None, edge_resolution: float = 0.01, interpolation_count: int = 0, validity_fn: StateValidityFn | None = None, objective: ob.OptimizationObjective | None = None, included_contact_geoms: Iterable[int] | None = None) → PathResult[source]

Plan a collision-free joint-space motion with OMPL and MuJoCo.

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-like) – RobotBlockSet MuJoCo robot object, or a composite object exposing a robots sequence.

  • q_start (array_like, shape (n,)) – Start joint configuration.

  • q_goal (array_like, shape (n,)) – Goal joint configuration.

  • algorithm (str or None, optional) – Planner name forwarded to JointPathPlanner. If None, OMPL’s default planner selection is used.

  • max_planning_time (float, optional) – Planner solve time budget in seconds.

  • max_simplification_time (float, optional) – Path-simplification time budget in seconds. The default 0.0 uses OMPL’s default simplification procedure.

  • clearance (float, optional) – Collision-margin inflation applied to robot geoms while planning.

  • only_robot_contacts (bool, optional) – If True, contacts not involving robot geoms are ignored.

  • excluded_contact_geom_ids (iterable of int, optional) – MuJoCo geom ids whose contacts are ignored by the default MuJoCo validity checker. This argument is not applied when a custom validity_fn is supplied.

  • excluded_contact_geom_pairs (iterable of pair-like, optional) – MuJoCo geom-id pairs whose contacts are ignored by the default MuJoCo validity checker. Pair order is ignored. This argument is not applied when a custom validity_fn is supplied.

  • edge_resolution (float, optional) – OMPL validity-checking resolution as a fraction of state-space extent.

  • interpolation_count (int, optional) – Number of path states after interpolation. Values greater than 3 enable interpolation; values 0 through 3 leave the simplified path unchanged.

  • validity_fn (callable, optional) – Custom OMPL state-validity callback. If omitted, MuJoCo contact checking is used.

  • objective (ompl.base.OptimizationObjective, optional) – Optimization objective for planners that require one.

  • included_contact_geoms (iterable of int, optional) – MuJoCo geom ids whose contacts always invalidate the state in the default MuJoCo validity checker. This argument is not applied when a custom validity_fn is supplied.

Returns:

  • path (ndarray, shape (m, n), or None) – Simplified and optionally interpolated solution path.

  • waypoints (ndarray, shape (k, n), or None) – Raw solution waypoints before simplification.

Raises:

ValueError – If joint dimensions do not match, max_simplification_time is negative, an excluded geom pair does not contain exactly two ids, or the requested planner is unavailable.

Functions

make_state_space(bounds)

Create an OMPL real-vector state space from numeric bounds.

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

Plan a collision-free joint-space motion with OMPL and MuJoCo.

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.

Classes

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

OMPL/MuJoCo joint-space path planner.