Usage

Installation

SRMP supports Ubuntu>=22.04 and Python 3.9, 3.10, 3.11, 3.12, and 3.13.

To use SRMP, first install it using pip:

(.venv) $ pip install srmp

Note

If you encounter installation issues, see the Troubleshooting Installation section below.

Then, import it in your script and follow the instructions on the docs:

import srmp
import numpy as np

Basic Single Robot Planning

Note

You can download pre-configured robot models (URDF/SRDF files) from our Data Downloads page.

Create a planner interface:

planner = srmp.PlannerInterface()

Add a robot model to the world:

Note

The srdf_path argument is optional. If you don’t have an SRDF file, omit the argument or pass an empty string (srdf_path=””).

# With SRDF (recommended when semantic info is available)
planner.add_articulation(name="panda",
                         end_effector="panda_hand",
                         urdf_path="/path/to/panda.urdf",
                         srdf_path="/path/to/panda.srdf")

# Or without SRDF (URDF only)
planner.add_articulation(name="panda",
                         end_effector="panda_hand",
                         urdf_path="/path/to/panda.urdf")

Add objects to the environment:

# Add a box obstacle
obstacle_pose = srmp.Pose()
obstacle_pose.p = np.array([0.5, 0.2, 0.4])
obstacle_size = np.array([0.1, 0.1, 0.4])
planner.add_box("box", obstacle_size, obstacle_pose)

# Add a mesh
mesh_pose = srmp.Pose()
mesh_pose.p = np.array([0.3, 0.3, 0.5])
planner.add_mesh("mesh_object", mesh_path="/path/to/mesh.stl",
                 scale=np.array([1.0, 1.0, 1.0]), pose=mesh_pose)

# Add a point cloud
# Generate sample point cloud data (Nx3 array)
point_cloud = np.random.rand(1000, 3) * 0.5 + np.array([0.2, 0.2, 0.3])
planner.add_point_cloud("point_cloud_obstacle", point_cloud, resolution=0.02)

Define the start configuration:

start_state = np.array([50, 47, -10, -35, -22, 93, 39])
start_state = np.radians(start_state)

Define goal constraints:

# Goal as joint angles
goal_state = np.array([21, 29, -30, -104, -162, 52, -118])
goal_state = np.radians(goal_state)
goal_joints = srmp.GoalConstraint(srmp.GoalType.JOINTS, [goal_state])

# Goal as end-effector pose
goal_pose = srmp.Pose()
goal_pose.p = np.array([0.642, -0.068, 0.505])
goal_pose.q = np.array([0.0, 0.0, 0.0, 1.0])  # normalized quaternion
goal_ee = srmp.GoalConstraint(srmp.GoalType.POSE, [goal_pose])

Configure the planner:

# Available planners and configurations
planner.print_available_planners()

# wAstar planner with BFS heuristic
planner.make_planner(["panda"], {"planner_id": "wAstar",
                                 "heuristic": "bfs",
                                 "weight": "10."})

# ARA* planner with parameters
planner.make_planner(["panda"], {"planner_id": "ARAstar",
                                 "heuristic": "bfs",
                                 "weight": "10.",
                                 "weight_delta": "1.",
                                 "final_weight": "1."})

# PA*SE
planner.make_planner(["panda"], {"planner_id": "wPASE",
                                 "heuristic": "bfs", "weight": "50.", "time_limit": "5.",
                                 "num_threads": "8"})

Compute a trajectory:

trajectory = planner.plan(start_state, goal_joints)
# or
trajectory = planner.plan(start_state, goal_ee)

# Access trajectory data
print(f"Trajectory length: {len(trajectory.positions)}")
for i, position in enumerate(trajectory.positions):
    print(f"Step {i}: {position}")

Multi-Robot Planning

Note

Multi-robot URDF/SRDF configurations are available for download on the Data Downloads page.

Add multiple robots to the world:

planner = srmp.PlannerInterface()

# Add first robot
planner.add_articulation(
    name="panda0",
    end_effector="panda0_hand",
    urdf_path="/path/to/panda0.urdf",
    srdf_path="/path/to/panda0.srdf"
)

# Add second robot
planner.add_articulation(
    name="panda1",
    end_effector="panda1_hand",
    urdf_path="/path/to/panda1.urdf",
    srdf_path="/path/to/panda1.srdf"
)

Set base poses for robots:

# Set base pose for panda0
pose0 = srmp.Pose()
pose0.p = np.array([-0.5, 0.5, 0])
pose0.q = np.array([1, 0, 0, 0])
planner.set_base_pose("panda0", pose0)

# Set base pose for panda1
pose1 = srmp.Pose()
pose1.p = np.array([0.5, 0.3, 0])
pose1.q = np.array([0, 0, 0, 1])
planner.set_base_pose("panda1", pose1)

Configure multi-robot planner:

articulation_names = ["panda0", "panda1"]
planner_context = {
    "planner_id": "xECBS",
    "weight_low_level_heuristic": "55.0",
    "high_level_focal_suboptimality": "1.8",
    "low_level_focal_suboptimality": "1.0",
    "heuristic_panda0": "joint_euclidean_remove_time",
    "heuristic_panda1": "joint_euclidean_remove_time",
}
   # Add robot-specific parameters similar to test_mramp.py
   for name in articulation_names:
      planner_context[f"heuristic_{name}"] = "joint_euclidean_remove_time"
      planner_context[f"mprim_path_{name}"] = "/path/to/config/manip_7dof_timed_mprim.yaml"

   planner.make_planner(articulation_names=articulation_names, planner_context=planner_context)

Define start and goal states for multiple robots:

start_states = {
    "panda0": np.radians([-40, 0, 0, -85, 0, 57, 0]),
    "panda1": np.radians([-40, 0, 0, -85, 0, 57, 0])
}

goal_states = {
    "panda0": np.radians([40, 0, 0, -70, 0, 50, 0]),
    "panda1": np.radians([40, 0, 0, -95, 0, 67, 0])
}

# Create goal constraints
goal_constraints = {}
for art_name in goal_states.keys():
    goal_constraints[art_name] = srmp.GoalConstraint(srmp.GoalType.JOINTS, [goal_states[art_name]])

Plan for multiple robots:

trajectories = planner.plan_multi(start_states, goal_constraints)

# Access individual robot trajectories
for robot_name, trajectory in trajectories.items():
    print(f"Robot {robot_name}: {len(trajectory.positions)} waypoints")

Simulator Integration

SRMP supports integration with multiple simulators. The planner can automatically read collision objects from the simulation environment:

Genesis Integration:

import genesis as gs

# Create Genesis scene
scene = gs.Scene()
# ... add objects to scene ...

# Read objects from Genesis
planner.read_sim(scene, "genesis")

PyBullet Integration:

import pybullet as p

# Create PyBullet simulation
physics_client = p.connect(p.GUI)
# ... add objects to simulation ...

# Read objects from PyBullet (exclude articulated bodies)
planner.read_sim(physics_client, "pybullet", articulations=["panda"])

SAPIEN Integration:

import sapien

# Create SAPIEN scene
scene = sapien.Scene()
# ... add objects to scene ...

# Read objects from SAPIEN
planner.read_sim(scene, "sapien")

Available Planners

SRMP provides several search-based planning algorithms:

wAstar: Weighted A* - Fast single-goal planning
ARAstar: Anytime Repairing A* - Iteratively improves solution quality
MHAstar: Multi-heuristic A* - Uses multiple heuristics for better performance
wPASE: Weighted PASE - Parallel search for improved performance
Astar: Standard A* - Optimal but potentially slower
E-CBS: Enhanced Conflict-Based Search - For multi-robot coordination
xECBS: Experience Accelerated Conflict-Based Search - For multi-robot coordination

You can view available planners programmatically:

planner.print_available_planners()

Environment Management

Remove objects from the environment:

planner.remove_object("box")

Supported geometric primitives:

Boxes: add_box(name, size, pose)
Spheres: add_sphere(name, radius, pose)
Cylinders: add_cylinder(name, radius, height, pose)
Meshes: add_mesh(name, mesh_path, scale, pose)
Point Clouds: add_point_cloud(name, vertices, resolution)

See the API for detailed method signatures and additional functionality.

Troubleshooting Installation

Conda Environment Issues

If you’re using conda and encounter C++ library compatibility issues:

$ conda install -c conda-forge libstdcxx-ng

Missing NumPy

If you get import errors related to NumPy:

$ pip install numpy