API Reference
Core Classes
PlannerInterface
The main interface for robot motion planning.
- class srmp.PlannerInterface
The primary class for creating and configuring planners.
Methods:
- add_articulation(name, end_effector,
- urdf_path, srdf_path='',
- link_names: List[str] = [],
- joint_names: List[str] = [],
- gravity: NDArray[np.float64] = np.array([0, 0, 0]),
- planned=True)
Add a robot to the planning scene. The srdf_path argument is optional — if you don’t have an SRDF file, you can omit this argument or pass an empty string.
- Parameters:
name (str) – Unique name for this robot instance
end_effector (str) – Name of the end-effector link
urdf_path (str) – Path to the URDF file describing the robot
srdf_path (str) – Path to the SRDF file with semantic information (optional, default: “”)
link_names (list) – List of link names to include (default: all links)
joint_names (list) – List of joint names to include (default: all joints)
gravity (numpy.ndarray) – Gravity vector for the robot (default: [0, 0, 0])
planned (bool) – Whether this robot should be planned for (default: True)
- remove_articulation(name)
Remove an articulation from the planning world.
- Parameters:
name (str) – Name of the articulation to remove
- set_base_pose(name, pose)
Set the base pose of a robot.
- make_planner(articulation_names, planner_context)
Configure and initialize the planner.
- Parameters:
Required Parameters:
planner_id(string): Planner algorithm to use
Single Robot Planner Context Options:
Available single-robot planners: “Astar”, “wAstar”, “ARAstar”, “MHAstar”, “wPASE”
General Parameters:
heuristic(string): Heuristic function (“bfs”, “joint_euclidean”, “joint_euclidean_remove_time”). Default: “bfs”resolution(string): Joint angle discretization in degrees. Default: “1”mprim_path(string): Path to motion primitives file. Default: auto-generated based on DOFtime_limitorallowed_planning_time(string): Planning time limit in seconds. Default: “10”
Planner-Specific Parameters:
- A* (“Astar”):
Uses only general parameters above.
- Weighted A* (“wAstar”):
weight(string): Heuristic weight. Default: “50”
- ARA* (“ARAstar”):
weight(string): Initial heuristic weight. Default: “50”weight_delta(string): Weight reduction per iteration. Default: “10.0”final_weight(string): Final weight to reach. Default: “1.0”
- MHA* (“MHAstar”):
heuristic(string): Anchor heuristic. Default: “joint_euclidean”inadmissible_heuristics(vector<string>): List of inadmissible heuristics. Default: [“bfs”]w1(string): Anchor heuristic weight. Default: “20”w2(string): Inadmissible heuristic weight. Default: “5”
- wPASE (“wPASE”):
heuristic(string): Primary heuristic. Default: “joint_euclidean”i_heuristic(string): Secondary heuristic. Default: “joint_euclidean”weight(string): Primary heuristic weight. Default: “50”i_weight(string): Secondary heuristic weight. Default: “100.0”num_threads(string): Number of parallel threads. Default: “4”
Multi-Robot Planner Context Options:
Available multi-robot planners: “E-CBS”, “xECBS”
Required Multi-Robot Parameters:
planner_id: “E-CBS” or “xECBS”
Agent-Specific Parameters (per robot):
For each robot with name
{robot_name}:heuristic_{robot_name}(string): Heuristic for this robot. Default: “bfs” (E-CBS), “joint_euclidean_remove_time” (xECBS)mprim_path_{robot_name}(string): Motion primitives path for this robot. Default: auto-generated timed versionresolution_{robot_name}(string): Discretization for this robot. Default: “1”
E-CBS/xECBS Parameters:
weight_low_level_heuristic(string): Low-level search weight. Default: “1.0” (E-CBS), “55.0” (xECBS)high_level_focal_suboptimality(string): High-level focal search bound. Default: “1.3”low_level_focal_suboptimality(string): Low-level focal search bound. Default: “1.3”
- plan(start, goal_constraint)
Plan a trajectory for a single robot.
- Parameters:
start (numpy.ndarray) – Starting joint configuration
goal_constraint (GoalConstraint) – Goal specification
- Returns:
Trajectory object containing the planned path
- Return type:
- plan_multi(start_states, goal_constraints)
Plan trajectories for multiple robots simultaneously.
- add_box(name, size, pose)
Add a box obstacle to the environment.
- add_sphere(name, radius, pose)
Add a sphere obstacle to the environment.
- add_cylinder(name, radius, height, pose)
Add a cylinder obstacle to the environment.
- add_mesh(name, mesh_path=None, vertices=None, triangles=None, scale=np.ones(3), pose=None, convex=False)
Add a mesh obstacle to the environment. Either
mesh_pathor bothverticesandtrianglesmust be provided.- Parameters:
name (str) – Unique name for the mesh
mesh_path (str) – Path to a mesh file (STL, OBJ, DAE, …). Mutually exclusive with
vertices/triangles.vertices (numpy.ndarray) – Mesh vertices as an Nx3 array. Must be supplied together with
triangles.triangles (numpy.ndarray) – Triangle face indices as an Mx3 array of integer indices into
vertices. Must be supplied together withvertices.scale (numpy.ndarray) – Uniform or per-axis scale factors [x, y, z] (default:
[1, 1, 1])pose (Pose) – Mesh pose in world frame
convex (bool) – Treat mesh as convex hull during collision checking. Only applicable for file-based meshes (default:
False)
- add_point_cloud(name, vertices, resolution=0.01)
Add a point cloud as a collision object to the environment.
- remove_object(name)
Remove an object from the environment.
- Parameters:
name (str) – Name of the object to remove
- read_sim(sim, sim_type, articulations=None)
Import objects from a simulation environment.
- print_available_planners()
Print available planners and their descriptions.
Note: add_articulation accepts an optional srdf_path parameter. Many examples below include SRDF paths for completeness, but you can call add_articulation with only the urdf_path if no SRDF is required for your use case.
- get_articulation_names()
Return the list of articulation names currently present in the planning world.
- Returns:
List of articulation names
- get_object_names()
Return the list of object names currently present in the planning world.
- Returns:
List of object names
- has_articulation(name)
Check whether an articulation with the given name exists in the planning world.
- Parameters:
name (str) – Articulation name
- Returns:
True if the articulation exists, False otherwise
- has_object(name)
Check whether an object with the given name exists in the planning world.
- Parameters:
name (str) – Object name
- Returns:
True if the object exists, False otherwise
- is_articulation_planned(name)
Check whether a named articulation is configured to be planned.
- Parameters:
name (str) – Articulation name
- Returns:
True if the articulation is planned, False otherwise
- set_articulation_planned(name, planned)
Enable or disable planning for a specific articulation.
- is_object_attached(name)
Query whether an object is currently attached to a robot.
- Parameters:
name (str) – Object name
- Returns:
True if attached, False otherwise
- attach_object(name, art_name, link_id, touch_links=None)
Attach an existing object to a robot link so it moves with the robot.
- detach_object(name, also_remove=False)
Detach an attached object from its robot. Optionally remove it from the world.
- detach_all_objects(also_remove=False)
Detach all attached objects. Optionally remove them from the world.
- Parameters:
also_remove (bool) – If True, remove detached objects from the world
- Returns:
True if successful
- is_state_colliding(articulation_name='')
Check whether the current state (optionally for a specific articulation) is in collision.
- Parameters:
articulation_name (str) – Optional articulation name to check
- Returns:
True if a collision is detected
- is_robot_colliding_with_objects(art_name)
Check if the specified robot is colliding with any objects in the planning world.
- Parameters:
art_name (str) – Articulation name
- Returns:
True if collision detected with environment objects
- distance_to_self_collision()
Get the minimum signed distance to self-collision for all robots.
- Returns:
Minimum self-collision distance (float)
- distance_to_robot_collision()
Get the minimum signed distance between robots and environment objects.
- Returns:
Minimum robot-to-environment collision distance (float)
- distance_to_collision()
Get the minimum distance to any collision (self or environment).
- Returns:
Minimum distance to collision (float)
- set_allowed_collision(name1, name2, allowed)
Set whether collisions between two named objects are allowed.
- set_qpos(name, qpos)
Set the joint positions for a named articulation.
- Parameters:
name (str) – Articulation name
qpos (numpy.ndarray) – Joint positions (1D array)
- set_qpos_all(state)
Set the joint positions for all planned articulations using a concatenated state vector.
- Parameters:
state (numpy.ndarray) – Concatenated joint positions for all planned articulations
- update_attached_bodies_pose()
Update the poses of all objects attached to robots based on current robot joint states.
- compute_fk(articulation_name, qpos)
Compute forward kinematics for a specific articulation and joint configuration.
- Parameters:
articulation_name (str) – Articulation name
qpos (numpy.ndarray) – Joint positions
- Returns:
Pose of the end-effector (Pose)
- compute_ik(articulation_name, ee_pose, init_state_val)
Compute inverse kinematics (CLIK) for a desired end-effector pose.
Data Types
Pose
GoalConstraint
GoalType
Trajectory
Motion Primitives Configuration
Motion primitives define the discrete actions available to the robot during planning. SRMP uses YAML configuration files to define motion primitive families and their properties.
YAML Configuration Files
File Structure:
<family_name>:
<primitive_name>:
mprim_sequence:
- [0, 0, 0, ...] # Always starts with origin (all zeros)
- [delta1, delta2, ...] # Delta values from origin
- [delta1, delta2, ...] # Additional steps (optional)
mprim_sequence_transition_costs: [cost1, cost2, 0] # Last is always 0
mprim_sequence_transition_times: [time1, time2, 0] # Optional timing
generate_negative: true/false # Whether to generate negative deltas
Key Components:
Family Name: Groups related primitives (e.g.,
long_primitives,short_primitives)Primitive Name: Unique identifier for each motion primitive
mprim_sequence: Sequence of states, always starting with zeros (origin)
mprim_sequence_transition_costs: Cost for each transition in the sequence
mprim_sequence_transition_times: Optional time constraints for each transition
generate_negative: Automatically creates negative versions of primitives
Units:
Joint space (manipulators): Degrees for angular movements
Example: 7DOF Manipulator Primitives
long_primitives:
joint0:
mprim_sequence:
- [0, 0, 0, 0, 0, 0, 0] # Origin state
- [15, 0, 0, 0, 0, 0, 0] # Move joint 0 by 15 degrees
mprim_sequence_transition_costs: [1, 0]
generate_negative: true
joint1:
mprim_sequence:
- [0, 0, 0, 0, 0, 0, 0]
- [0, 15, 0, 0, 0, 0, 0] # Move joint 1 by 15 degrees
mprim_sequence_transition_costs: [1, 0]
generate_negative: true
short_primitives:
joint0:
mprim_sequence:
- [0, 0, 0, 0, 0, 0, 0]
- [7, 0, 0, 0, 0, 0, 0] # Move joint 0 by 7 degrees
mprim_sequence_transition_costs: [1, 0]
generate_negative: true
Example: Timed Motion Primitives
long_primitives:
joint0:
mprim_sequence:
- [0, 0, 0, 0, 0, 0, 0]
- [15, 0, 0, 0, 0, 0, 0]
mprim_sequence_transition_costs: [1, 0]
mprim_sequence_transition_times: [1, 0] # 1 time unit per transition
generate_negative: true
Creating Custom Motion Primitives
Step 1: Define YAML Configuration
Create a new YAML file with appropriate naming (e.g.,
custom_7dof_mprim.yaml)Define motion primitive families based on your robot’s requirements
Specify primitives for each joint
Set appropriate costs and timing constraints
Step 2: File Naming Convention
Manipulators:
<robot_type>_<dof>dof[_additional_info]_mprim.yamlTimed variants: Include
_timedin the filename for multi-robot coordination
Step 3: Integration with Planner
# Use custom motion primitives
planner.make_planner(["robot_name"], {
"planner_id": "wAstar",
"heuristic": "bfs",
"mprim_path": "/path/to/custom_7dof_mprim.yaml"
})
# For multi-robot with custom primitives per robot
planner.make_planner(["robot1", "robot2"], {
"planner_id": "xECBS",
"mprim_path_robot1": "/path/to/robot1_timed_mprim.yaml",
"mprim_path_robot2": "/path/to/robot2_timed_mprim.yaml"
})
Design Guidelines:
Start Simple: Begin with single-joint movements before complex combinations
Balance Resolution vs Speed: More primitives = finer control but slower planning
Cost Weighting: Use costs to prefer certain types of movements
Symmetric Movements: Use
generate_negative: truefor symmetric joint movementsMulti-Robot: Use timed primitives (
_timed_mprim.yaml) for coordination
Available Primitive Files:
The SRMP package includes pre-configured motion primitive files for manipulators:
manip_6dof_mprim.yaml- 6DOF manipulator primitivesmanip_7dof_mprim.yaml- 7DOF manipulator primitivesmanip_7dof_timed_mprim.yaml- 7DOF with timing for multi-robot coordination
Examples
Basic Single Robot Example
import srmp
import numpy as np
# Create planner
planner = srmp.PlannerInterface()
# Add robot
planner.add_articulation(
urdf_path="/path/to/panda.urdf",
srdf_path="/path/to/panda.srdf",
name="panda",
end_effector="panda_hand"
)
# Add obstacle
obstacle_pose = srmp.Pose()
obstacle_pose.p = np.array([0.5, 0.2, 0.4])
planner.add_box("obstacle", np.array([0.1, 0.1, 0.4]), obstacle_pose)
# Configure planner
planner.make_planner(["panda"], {
"planner_id": "wAstar",
"heuristic": "bfs",
"weight": "10.0"
})
# Plan trajectory
start_state = np.radians([0, -45, 0, -135, 0, 90, 45])
goal_pose = srmp.Pose()
goal_pose.p = np.array([0.6, 0.0, 0.5])
goal_pose.q = np.array([0, 0, 0, 1])
goal = srmp.GoalConstraint(srmp.GoalType.POSE, [goal_pose])
trajectory = planner.plan(start_state, goal)
Multi-Robot Example
import srmp
import numpy as np
# Create planner
planner = srmp.PlannerInterface()
# Add two robots
for i in range(2):
planner.add_articulation(
urdf_path=f"/path/to/panda{i}.urdf",
srdf_path=f"/path/to/panda{i}.srdf",
name=f"panda{i}",
end_effector=f"panda{i}_hand"
)
# Set base poses
for i in range(2):
pose = srmp.Pose()
pose.p = np.array([(-1)**i * 0.5, 0.5, 0])
pose.q = np.array([1, 0, 0, 0])
planner.set_base_pose(f"panda{i}", pose)
# Configure multi-robot planner (add per-robot heuristic and mprim paths like tests)
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",
}
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)
# Plan trajectories
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])
}
goal_constraints = {}
for name, goal_state in goal_states.items():
goal_constraints[name] = srmp.GoalConstraint(srmp.GoalType.JOINTS, [goal_state])
trajectories = planner.plan_multi(start_states, goal_constraints)
Simulator Integration Example
import srmp
import sapien
import numpy as np
# Create SAPIEN scene
scene = sapien.Scene()
scene.add_ground()
# Add some objects to scene
builder = scene.create_actor_builder()
builder.add_box_collision(half_size=[0.1, 0.1, 0.1])
builder.add_box_visual(half_size=[0.1, 0.1, 0.1])
box = builder.build_kinematic()
box.set_pose(sapien.Pose([0.5, 0.0, 0.5]))
# Create planner
planner = srmp.PlannerInterface()
# Add robot
planner.add_articulation(
urdf_path="/path/to/panda.urdf",
srdf_path="/path/to/panda.srdf",
name="panda",
end_effector="panda_hand"
)
# Import scene objects automatically
planner.read_sim(scene, "sapien")
# Continue with planning as usual...
Point Cloud Example
import srmp
import numpy as np
# Create planner
planner = srmp.PlannerInterface()
# Add robot
planner.add_articulation(
urdf_path="/path/to/panda.urdf",
srdf_path="/path/to/panda.srdf",
name="panda",
end_effector="panda_hand"
)
# Load point cloud from file (example formats: PLY, PCD, or custom)
# For this example, we'll generate a synthetic point cloud
# Generate a point cloud representing a table surface
table_points = []
for x in np.linspace(0.2, 0.8, 30):
for y in np.linspace(-0.3, 0.3, 20):
table_points.append([x, y, 0.4]) # Table at height 0.4m
table_cloud = np.array(table_points)
# Generate a point cloud representing a wall
wall_points = []
for y in np.linspace(-0.5, 0.5, 40):
for z in np.linspace(0.0, 1.5, 60):
wall_points.append([0.9, y, z]) # Wall at x=0.9m
wall_cloud = np.array(wall_points)
# Add point clouds to planner with different resolutions
planner.add_point_cloud("table_surface", table_cloud, resolution=0.01)
planner.add_point_cloud("wall", wall_cloud, resolution=0.02)
# Configure planner
planner.make_planner(["panda"], {
"planner_id": "wAstar",
"heuristic": "bfs",
"weight": "10.0"
})
# Plan around point cloud obstacles
start_state = np.radians([0, -45, 0, -135, 0, 90, 45])
# Goal pose that requires navigating around the point cloud obstacles
goal_pose = srmp.Pose()
goal_pose.p = np.array([0.7, 0.1, 0.6]) # Above the table, near the wall
goal_pose.q = np.array([0, 0, 0, 1])
goal = srmp.GoalConstraint(srmp.GoalType.POSE, [goal_pose])
trajectory = planner.plan(start_state, goal)
if trajectory:
print(f"Successfully planned around point cloud obstacles")
print(f"Trajectory length: {len(trajectory.positions)} waypoints")
else:
print("Planning failed - point cloud obstacles may block all paths")
# Point cloud loading from files (common formats)
def load_point_cloud_from_ply(filename):
"""Load point cloud from PLY file"""
# This is a simplified example - use libraries like Open3D for robust loading
points = []
with open(filename, 'r') as f:
lines = f.readlines()
# Skip PLY header, find vertex data
vertex_start = False
for line in lines:
if line.strip() == "end_header":
vertex_start = True
continue
if vertex_start and line.strip():
coords = line.strip().split()
if len(coords) >= 3:
points.append([float(coords[0]), float(coords[1]), float(coords[2])])
return np.array(points)
def load_point_cloud_from_txt(filename):
"""Load point cloud from simple text file (x y z per line)"""
return np.loadtxt(filename)
# Usage with file loading
# point_cloud = load_point_cloud_from_ply("/path/to/scan.ply")
# planner.add_point_cloud("scanned_object", point_cloud, resolution=0.005)
# Point cloud from sensor data (example with simulated LiDAR-style data)
def generate_lidar_point_cloud(robot_pose, num_rays=360, max_range=5.0):
"""Generate simulated LiDAR point cloud"""
points = []
for i in range(num_rays):
angle = 2 * np.pi * i / num_rays
# Simulate ray hitting objects at various distances
distance = np.random.uniform(0.5, max_range)
x = robot_pose[0] + distance * np.cos(angle)
y = robot_pose[1] + distance * np.sin(angle)
z = robot_pose[2] + np.random.uniform(-0.1, 0.1) # Some height variation
points.append([x, y, z])
return np.array(points)
# Simulate sensor-based point cloud
robot_position = [0, 0, 0.5]
sensor_cloud = generate_lidar_point_cloud(robot_position)
planner.add_point_cloud("sensor_obstacles", sensor_cloud, resolution=0.03)
Visualization Classes
SRMP ships two optional visualization backends. Both extend
PlannerInterface and keep the 3D scene in sync automatically
as robots and objects are added or removed.
See the Visualization page for installation instructions and full usage examples.
VisualPlannerInterface
- class srmp.VisualPlannerInterface(zmq_url=None)
MeshCat-based visualizer. Inherits all methods of
PlannerInterface.- Parameters:
zmq_url (str) – Optional ZMQ URL for an existing MeshCat server. Leave as
Noneto start a new server automatically.
Requires:
pip install meshcatAdditional Methods:
- visualize(open_browser=True)
Start the MeshCat server (if not already running) and render the current scene. Prints the browser URL to the console.
- Parameters:
open_browser (bool) – Whether to print the URL (default:
True)
- animate_trajectory(trajectories, dt=0.05, robot_name=None)
Replay a planned trajectory in the 3D viewer by stepping through joint configurations.
- Parameters:
trajectories – A single
Trajectoryor adictmapping robot names to trajectories.dt (float) – Seconds to wait between frames (default:
0.05)robot_name (str) – Robot name when a single trajectory is supplied. Defaults to the first robot in the scene.
- add_gui_controls()
Add obstacle-editing sliders and controls to MeshCat. Because MeshCat is one-directional, use the
set_gui_*helpers below to drive the GUI state from Python.
- set_gui_position(x, y, z)
Set the current GUI position state and sync the MeshCat sliders.
- set_gui_size(width, height, depth)
Set the current GUI size state and sync the MeshCat sliders.
- set_gui_object_type(obj_type)
Set the current GUI object type (
"box","sphere", or"cylinder").
- set_gui_object_name(name)
Set the current GUI object name.
- add_obstacle_from_gui()
Add an obstacle using the current GUI state values.
- Returns:
Name of the added object, or
Noneon failure.- Return type:
str or None
- update_object_from_gui(object_name)
Move an existing object to the position stored in the current GUI state.
- load_object_to_gui(object_name)
Load an object’s properties into the GUI state (for subsequent editing).
- url
The MeshCat browser URL (read-only). Returns an informational string if the server has not been started yet.
ViserPlannerInterface
- class srmp.ViserPlannerInterface(port=8080, share=False)
Interactive Viser-based visualizer. Inherits all methods of
PlannerInterface. Unlike the MeshCat backend, Viser provides bidirectional browser ↔ Python communication: sliders, dropdowns, and buttons in the browser directly invoke Python callbacks.- Parameters:
Requires:
pip install viser trimeshAdditional Methods:
- visualize(open_browser=True, add_grid=True)
Start the Viser server (if not already running) and render the scene.
- animate_trajectory(trajectories, dt=0.05, robot_name=None)
Replay a planned trajectory in the 3D viewer.
- Parameters:
trajectories – A single
Trajectoryor adictmapping robot names to trajectories.dt (float) – Seconds between frames (default:
0.05)robot_name (str) – Robot name when a single trajectory is supplied.
- add_robot_controls(robot_name)
Add interactive joint sliders, visibility toggles, and a Reset button for the named robot. Moving a slider updates both the Viser geometry and the planner backend in real time.
- Parameters:
robot_name (str) – Name of the robot
- add_ee_drag_control(robot_name)
Place a 6-DOF transform gizmo at the robot’s end-effector. Dragging the gizmo in the browser triggers IK; on success the joint configuration is updated. On IK failure the gizmo snaps back to the current end-effector pose. If
add_robot_controls()was called first, the joint sliders are kept in sync.- Parameters:
robot_name (str) – Name of the robot
- add_gui_controls()
Add an interactive “Object Controls” panel to the browser sidebar. The panel includes type dropdown, position vector, size sliders, mesh path input, file-browser button, and Add / Update / Remove buttons that invoke Python callbacks.
- add_obstacle_from_gui()
Add an obstacle using the current GUI widget values.
- Returns:
Name of the added object, or
Noneon failure.- Return type:
str or None
- update_object_from_gui(object_name)
Move an existing object to the position shown in the GUI.
- load_object_to_gui(object_name)
Load an object’s properties into the GUI widgets for editing.
- stop()
Shut down the Viser web server.
- url
The Viser browser URL (e.g.
http://localhost:8080). Returns an informational string if the server has not been started yet.