Implementation:Haosulab ManiSkill BaseMotionPlanningSolver

Field	Value
Implementation Name	BaseMotionPlanningSolver
Type	API Doc
Domain	Motion_Planning
Source File	`mani_skill/examples/motionplanning/base_motionplanner/motionplanner.py` (L11-195)
Date	2026-02-15
Repository	Haosulab/ManiSkill

Overview

The BaseMotionPlanningSolver class provides the core motion planning interface for generating collision-free robot arm trajectories in ManiSkill environments. It wraps the mplib (Motion Planning Library) planner and exposes three planning strategies: screw motion, RRTConnect, and RRT*. Task-specific solvers inherit from or compose this class to implement manipulation sequences.

Description

The class manages the lifecycle of an mplib planner, initializing it from the robot's URDF/SRDF files and configuring joint velocity and acceleration limits. It provides methods to plan paths to target end-effector poses and to execute those paths by stepping the environment with joint position actions.

The class also supports point cloud-based collision avoidance, enabling planners to reason about obstacles that are not part of the robot model (e.g., table surfaces, other objects in the scene).

Usage

from mani_skill.examples.motionplanning.base_motionplanner.motionplanner import BaseMotionPlanningSolver
import sapien

# Typically used inside a task-specific solver:
planner = BaseMotionPlanningSolver(env, debug=False, vis=False)
target_pose = sapien.Pose(p=[0.3, 0.0, 0.2], q=[1, 0, 0, 0])
result = planner.move_to_pose_with_screw(target_pose)
if result == -1:
    # Screw motion failed, try RRTConnect
    result = planner.move_to_pose_with_RRTConnect(target_pose)

Code Reference

Class Signature

class BaseMotionPlanningSolver:
    def __init__(
        self,
        env: BaseEnv,
        debug: bool = False,
        vis: bool = True,
        base_pose: sapien.Pose = None,
        print_env_info: bool = True,
        joint_vel_limits: float = 0.9,
        joint_acc_limits: float = 0.9,
    ):

Constructor Parameters

Parameter	Type	Default	Description
`env`	`BaseEnv`	(required)	The ManiSkill environment instance (may be wrapped).
`debug`	`bool`	`False`	Enable debug mode with interactive stepping.
`vis`	`bool`	`True`	Enable GUI visualization during execution.
`base_pose`	`sapien.Pose`	`None`	Base pose of the robot (currently must be origin).
`print_env_info`	`bool`	`True`	Print step-by-step reward and info during execution.
`joint_vel_limits`	`float`	`0.9`	Scaling factor for joint velocity limits (0 to 1).
`joint_acc_limits`	`float`	`0.9`	Scaling factor for joint acceleration limits (0 to 1).

Key Methods

def setup_planner(self) -> mplib.Planner:
    """Initialize the mplib planner from robot URDF/SRDF, set base pose and joint limits."""

def follow_path(self, result: dict, refine_steps: int = 0):
    """Execute a planned path by stepping the environment with joint position actions.
    Args:
        result: Planning result dict with 'position' and optionally 'velocity' arrays.
        refine_steps: Extra steps to hold the final configuration for settling.
    Returns:
        (obs, reward, terminated, truncated, info) from the last env.step().
    """

def move_to_pose_with_screw(self, pose: sapien.Pose, dry_run: bool = False, refine_steps: int = 0):
    """Plan and execute a screw motion to the target pose.
    Retries once on failure. Returns -1 if planning fails."""

def move_to_pose_with_RRTConnect(self, pose: sapien.Pose, dry_run: bool = False, refine_steps: int = 0):
    """Plan and execute an RRTConnect motion to the target pose.
    Returns -1 if planning fails."""

def move_to_pose_with_RRTStar(self, pose: sapien.Pose, dry_run: bool = False, refine_steps: int = 0):
    """Plan and execute an RRT* motion to the target pose with 1s planning time.
    Returns -1 if planning fails."""

def add_box_collision(self, extents: np.ndarray, pose: sapien.Pose):
    """Add a box-shaped obstacle to the planner's point cloud collision model."""

def add_collision_pts(self, pts: np.ndarray):
    """Add arbitrary point cloud data as collision obstacles."""

def clear_collisions(self):
    """Remove all point cloud collision data from the planner."""

Planner Setup (L55-69)

def setup_planner(self):
    move_group = self.MOVE_GROUP if hasattr(self, "MOVE_GROUP") else "eef"
    link_names = [link.get_name() for link in self.robot.get_links()]
    joint_names = [joint.get_name() for joint in self.robot.get_active_joints()]
    planner = mplib.Planner(
        urdf=self.env_agent.urdf_path,
        srdf=self.env_agent.urdf_path.replace(".urdf", ".srdf"),
        user_link_names=link_names,
        user_joint_names=joint_names,
        move_group=move_group,
    )
    planner.set_base_pose(np.hstack([self.base_pose.p, self.base_pose.q]))
    planner.joint_vel_limits = np.asarray(planner.joint_vel_limits) * self.joint_vel_limits
    planner.joint_acc_limits = np.asarray(planner.joint_acc_limits) * self.joint_acc_limits
    return planner

I/O Contract

Direction	Data	Format
Input	Target end-effector pose	`sapien.Pose` (position [x,y,z] + quaternion [w,x,y,z])
Input	Current robot joint positions	Read internally via `self.robot.get_qpos()`
Output (dry_run=True)	Planning result	`dict` with keys `"position"`, `"velocity"`, `"status"`
Output (dry_run=False)	Environment step result	`(obs, reward, terminated, truncated, info)` tuple
Output (failure)	Failure indicator	`-1` (integer)

External Dependencies

Package	Purpose
mplib	Motion planning: IK solving, RRT, screw planning
sapien	Physics simulation, pose representation
trimesh	Box mesh creation and surface sampling for collision point clouds
numpy	Array operations for joint positions and poses

Usage Examples

# Inside a task-specific solver function (e.g., solvePickCube):
import sapien
import numpy as np

def solvePickCube(env, seed=None, debug=False, vis=False):
    env.reset(seed=seed)
    planner = PandaArmMotionPlanningSolver(env, debug=debug, vis=vis)

    # Add table as collision obstacle
    planner.add_box_collision(extents=np.array([1.0, 1.0, 0.01]),
                              pose=sapien.Pose(p=[0, 0, 0.0]))

    # Move to pre-grasp pose
    pre_grasp = sapien.Pose(p=[cube_pos[0], cube_pos[1], cube_pos[2] + 0.1],
                            q=grasp_quat)
    res = planner.move_to_pose_with_screw(pre_grasp)
    if res == -1:
        return -1

    # Close gripper, lift, etc.
    # ...

Related Pages

Page Connections

Double-click a node to navigate. Hold to expand connections.

Principle

Implementation

Heuristic

Environment