Grasp Planner
SUMMARY
Grasp Planner (GraspPlanner) checks the motion around a grasp. Where Grasp Filter only checks that the grasp and pregrasp poses can be reached, the planner moves along Cartesian straight lines between them and checks every step for collisions.
It can check the approach (pregrasp to grasp), the gripper closing at the grasp, or the full approach, lift and retreat sequence. It plans and checks motions only: it does not move the robot or the gripper.
UNITS
Candidate poses are 4x4 transforms in the robot base frame, translation in meters. Joint trajectories and grasp_joint_positions_deg are in degrees. Distances (approach_distance_m, lift_distance_m, retreat_distance_m) are in meters. Gripper configurations use the gripper model's native joint units: meters for prismatic joints and radians for revolute joints.
The Skill
data = grasps.GraspData()
# ... set tcp_frame, gripper_object_name, distances and the other fields below ...
plans = grasps.GraspPlanner().plan_all_approach_lift_retreat(
grasp_candidates=candidates,
grasp_filter_results=filter_results,
planning_scene=scene,
grasp_data=data,
)GraspPlanner, GraspData and GraspPlanResult load the native planner the first time you access them on grasps, so the first use is slightly slower.
The Code
"""
Plans the approach, gripper closing, lift and retreat of filtered grasps, and replays the paths in Rerun.
Builds an Epson CX4 with a Robotiq Hand-E in a planning scene, filters five
grasp poses, then plans motions around them with each GraspPlanner method.
Runs without a robot connection; the first run downloads the robot and gripper
models.
Supports Epson and virtual.
Usage:
python grasp_planner.py
python grasp_planner.py --save-rrd grasp_planner.rrd
"""
import argparse
import numpy as np
import rerun as rr
import rerun.blueprint as rrb
from loguru import logger
from telekinesis.synapse import grasps, planning_scene
from telekinesis.synapse.robots.manipulators import epson
from telekinesis.synapse.tools.parallel_grippers import robotiq
PRE_GRASP_OFFSET_M = 0.05
def init_rerun(app_id: str, save_rrd: str | None) -> "rr.RecordingStream":
"""Start a Rerun recording, live in the viewer or saved to an .rrd file."""
rr.init(app_id, spawn=save_rrd is None)
recording = rr.get_global_data_recording()
if save_rrd:
recording.save(save_rrd)
return recording
def main(save_rrd: str | None) -> None:
"""Filter five grasps, plan the motions around the ones that survive and replay them."""
#===================== Create Robot and Gripper ============================
robot = epson.EpsonCX4A601S()
gripper = robotiq.RobotiqHandE()
robot.attach_tool(gripper, mount_frame=robot.default_tcp)
robot.add_tcp(name="grasp_tcp", transform=[0, 0, 0.15, 0, 0, 0], set_active=True)
#===================== Create the Planning Scene ===========================
# The scene starts with the gripper open, so checks use its open shape.
scene = planning_scene.PlanningScene(robot.kinematic_model)
open_configuration = gripper.get_model_configuration_at_opening_width(40, unit="mm")[:1]
closed_configuration = gripper.get_model_configuration_at_opening_width(0, unit="mm")[:1]
scene.attach_collision_object(
"gripper",
parent_frame=robot.default_tcp,
touch_links=[robot.default_tcp, "j5_link", "j6_link"],
parent_T_object=np.eye(4),
urdf_path=gripper.urdf_path,
package_dir=gripper.model_dir,
configuration=open_configuration,
)
#===================== Create Candidate Grasps =============================
# A candidate is a dictionary with a 4x4 "base_T_pick" in the robot base frame.
seed_joints_deg = np.array([15, -35, 45, 10, 20, -10], dtype=float)
base_T_center = robot.kinematic_model.forward_kinematics(
np.deg2rad(seed_joints_deg), robot.active_tcp
)
candidates = []
for offset_y in np.linspace(-0.02, 0.02, 5):
base_T_pick = base_T_center.copy()
base_T_pick[:3, 3] += base_T_center[:3, 1] * offset_y
candidates.append({"base_T_pick": base_T_pick})
# The planner needs the filter's joint solutions, so filter with a pregrasp offset.
filter_results = grasps.GraspFilter().filter_grasps(
grasp_candidates=[candidate["base_T_pick"] for candidate in candidates],
planning_scene=scene,
ik_settings=robot.get_kinematics_settings(),
ik_frame=robot.active_tcp,
ik_solver_frame=robot.default_tcp,
ik_seed_joint_positions_deg=seed_joints_deg,
ik_joint_names=robot.joint_names,
pre_grasp_offset_m=PRE_GRASP_OFFSET_M,
)
#===================== Create the Grasp Data ===============================
# Settings shared by every candidate.
data = grasps.GraspData()
data.tcp_frame = robot.active_tcp
data.ik_solver_frame = robot.default_tcp
data.attachment_frame = robot.default_tcp
data.gripper_object_name = "gripper"
data.ee_link_names = [robot.default_tcp]
data.touch_links = [robot.default_tcp]
data.open_gripper_configuration = open_configuration
data.closed_gripper_configuration = closed_configuration
data.approach_direction_tcp = np.array([0.0, 0.0, 1.0])
data.approach_distance_m = PRE_GRASP_OFFSET_M
data.lift_distance_m = 0.05
data.retreat_direction_tcp = np.array([0.0, 0.0, -1.0])
data.retreat_distance_m = 0.05
data.ik_settings = robot.get_kinematics_settings()
# ==================== Run Skill ============================================
planner = grasps.GraspPlanner()
# 1. Approach only: pregrasp to grasp.
approaches = planner.plan_all_approaches(
grasp_candidates=candidates,
grasp_filter_results=filter_results,
planning_scene=scene,
grasp_data=data,
)
for plan in approaches:
logger.info(
f"Approach {plan.candidate_index}: accepted={plan.accepted}, "
f"fraction={plan.approach_fraction:.2f}, reason={plan.rejection_reason!r}"
)
# 2. Approach, then close the gripper at the grasp. This scene has no target
# occupancy in its OctoMap, so it is passed for both scenes.
closed = planner.plan_all_approach_and_close(
grasp_candidates=candidates,
grasp_filter_results=filter_results,
planning_scene=scene,
target_excluded_scene=scene,
grasp_data=data,
)
for plan in closed:
logger.info(
f"Approach and close {plan.candidate_index}: accepted={plan.accepted}, "
f"closing fraction={plan.gripper_closing_fraction:.2f}"
)
# 3. Gripper closing alone, with the arm fixed at a known grasp configuration.
for result in filter_results:
if result.is_valid:
close_plan = planner.plan_close(
target_excluded_scene=scene,
grasp_data=data,
grasp_joint_positions_deg=result.joint_positions_deg,
)
logger.info(
f"Close {result.candidate_index}: accepted={close_plan.accepted}, "
f"colliding={close_plan.closing_colliding_geometry_names}"
)
break
# 4. The full sequence: approach, lift and retreat.
plans = planner.plan_all_approach_lift_retreat(
grasp_candidates=candidates,
grasp_filter_results=filter_results,
planning_scene=scene,
grasp_data=data,
)
for plan in plans:
logger.info(
f"Candidate {plan.candidate_index}: filter={filter_results[plan.candidate_index].status.name}, "
f"accepted={plan.accepted}, fractions=({plan.approach_fraction:.2f}, "
f"{plan.lift_fraction:.2f}, {plan.retreat_fraction:.2f}), reason={plan.rejection_reason!r}"
)
if plan.accepted:
logger.success(
f" lift path: {len(plan.lift_joint_trajectory_deg)} states, "
f"last joints [deg]: {np.round(plan.lift_joint_trajectory_deg[-1], 2)}"
)
# ==================== Visualization ========================================
# Every candidate as a point (green = accepted, red = rejected), the filter's candidate
# and pregrasp arrows, then a replay of the approach, lift and retreat paths of the full
# sequence. The gripper is shown open on the approach and closed on the lift and retreat.
# The replay only plays planned states. It does not move any robot.
recording = init_rerun("telekinesis_synapse_grasp_planner", save_rrd)
recording.send_blueprint(
rrb.Blueprint(
rrb.Horizontal(
rrb.Spatial3DView(origin="/"),
rrb.TextLogView(origin="/grasp_planner"),
column_shares=[3, 1],
),
rrb.TimePanel(timeline="grasp_planning_step", play_state="following"),
)
)
recording.log("/", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True)
scene.visualize_rerun(recording_stream=recording, retain_collision_checks=True)
poses = [candidate["base_T_pick"] for candidate in candidates]
visualizer = grasps.GraspFilterVisualizer()
visualizer.visualize_grasps(grasp_poses=poses, results=filter_results, planning_scene=scene)
visualizer.visualize_pregrasp_poses(
grasp_poses=poses,
results=filter_results,
planning_scene=scene,
pre_grasp_offset_m=PRE_GRASP_OFFSET_M,
)
recording.log(
"/grasp_planner/candidates",
rr.Points3D(
positions=[pose[:3, 3] for pose in poses],
colors=[(0, 200, 90) if plan.accepted else (220, 60, 60) for plan in plans],
labels=[
f"candidate {index}: {'accepted' if plan.accepted else 'rejected'}"
for index, plan in enumerate(plans)
],
),
static=True,
)
# Each segment holds joint positions in degrees. A rejected plan may still hold a
# partial path, which is useful to inspect. Candidates the filter rejected have none.
sequence = 0
for plan in plans:
for segment_name, trajectory in (
("APPROACH", plan.approach_joint_trajectory_deg),
("LIFT", plan.lift_joint_trajectory_deg),
("RETREAT", plan.retreat_joint_trajectory_deg),
):
configuration = open_configuration if segment_name == "APPROACH" else closed_configuration
scene.set_attached_object_configuration("gripper", configuration)
for step, joints_deg in enumerate(trajectory):
recording.set_time("grasp_planning_step", sequence=sequence)
sequence += 1
scene.visualize_configuration_rerun(joints_deg, [])
recording.log(
"/grasp_planner/status",
rr.TextLog(
f"candidate={plan.candidate_index} "
f"{'ACCEPTED' if plan.accepted else 'REJECTED'} "
f"segment={segment_name} step={step + 1}/{len(trajectory)} "
f"joints_deg={np.round(joints_deg, 2).tolist()}"
),
)
scene.set_attached_object_configuration("gripper", open_configuration)
recording.flush()
if save_rrd:
logger.info(f"Saved Rerun recording: {save_rrd}")
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Synapse grasp planner example")
parser.add_argument("--save-rrd", type=str, default=None,
help="Save the Rerun recording to this .rrd file instead of opening the viewer")
args = parser.parse_args()
main(save_rrd=args.save_rrd)Rerun output
The viewer shows a point per candidate (green accepted, red rejected), the filter's candidate and pregrasp arrows, and a replay of each candidate's approach, lift and retreat paths on the grasp_planning_step timeline, with the gripper open on the approach and closed on the lift and retreat. The replay only plays planned states and never moves a robot. Pass --save-rrd out.rrd to save the recording to a file instead of opening the viewer, then open it later with rerun out.rrd.
Running the Example
python grasp_planner.pyTo save the Rerun recording to a file instead of opening the viewer:
python grasp_planner.py --save-rrd grasp_planner.rrdFor all options:
python grasp_planner.py --helpWhat Each Method Checks
| Method | Checks | Result fields set |
|---|---|---|
plan_all_approaches | A straight Cartesian path from the filter's pregrasp solution to the grasp pose, with the gripper open. Accepted only if the whole path is completed without a collision. | approach_fraction, approach_joint_trajectory_deg, and on a collision approach_collision_joint_positions_deg and approach_colliding_geometry_names. |
plan_all_approach_and_close | The approach above, then the gripper closing from open to closed at the arm's final approach state. | The approach fields, plus gripper_closing_trajectory, gripper_closing_fraction and closing_colliding_geometry_names. |
plan_close | Only the gripper closing, from open to closed, with the arm fixed at grasp_joint_positions_deg. No arm path. | gripper_closing_trajectory, gripper_closing_fraction, closing_colliding_geometry_names. |
plan_all_approach_lift_retreat | Approach with the gripper open, then lift and retreat with the gripper closed (and the target object attached, if you name one). | approach_*, lift_* and retreat_* fractions and joint trajectories. |
The lift moves the grasp pose straight up by lift_distance_m along the robot base Z axis. The retreat then moves from the lift pose along retreat_direction_tcp, expressed in the grasp TCP's local frame, by retreat_distance_m.
plan_all_approach_lift_retreat accepts a candidate when the approach path is complete, the product of the lift and retreat fractions is at least 0.90, and no target occupancy remains in the OctoMap.
Result list semantics
- Every
plan_all_*method returns oneGraspPlanResultper input, in input order, including rejected candidates.result.candidate_indexequals the position in the input list. grasp_candidates,grasp_filter_resultsand (if given)grasp_object_idsmust have the same length. Each filter result'scandidate_indexmust equal its position. Otherwise aValueErroris raised.- A candidate whose filter status is not
VALIDis not planned. Its result hasaccepted == Falseandrejection_reason == "grasp_filter_rejected". - A
VALIDfilter result must hold both joint solutions, so run the filter withpre_grasp_offset_m, or the planner raises aValueError. - Candidate poses must be valid rigid transforms. Poses stored as
float32can carry enough rotation error to be rejected with aValueError; re-orthonormalize them first.
Parameter Configuration
plan_all_approaches, plan_all_approach_lift_retreat
All arguments are keyword-only.
| Parameter | Type | Default | Description |
|---|---|---|---|
grasp_candidates | Sequence[dict] | required | One dictionary per candidate, with a 4x4 base_T_pick (the grasp TCP pose in the robot base frame). Other keys are ignored and the dictionaries are not modified. |
grasp_filter_results | Sequence[GraspFilterResult] | required | The Grasp Filter results for the same batch, aligned by position. |
planning_scene | PlanningScene | required | Scene with the robot, the attached gripper and the obstacles. |
grasp_data | GraspData | required | Frames, gripper configurations and motion settings. See below. |
grasp_object_ids | Sequence[str] | None | None | Optional world object ID per candidate. An empty string leaves the target unattached. A named object allows contact with ee_link_names, and in the full sequence is attached to the gripper for the lift and retreat. |
plan_all_approach_and_close
Takes the same arguments, plus:
| Parameter | Type | Default | Description |
|---|---|---|---|
target_excluded_scene | PlanningScene | required | A scene whose OctoMap omits the target's occupied cells but keeps the surrounding obstacles. If there is no target occupancy, pass the same scene as planning_scene. Both scenes must use the same robot model. |
plan_close
All arguments are keyword-only.
| Parameter | Type | Default | Description |
|---|---|---|---|
target_excluded_scene | PlanningScene | required | Scene whose OctoMap omits the target's occupancy. |
grasp_data | GraspData | required | Needs the gripper object, the open and closed configurations and gripper_closing_steps. |
grasp_joint_positions_deg | Sequence[float] | required | Arm joint positions at the grasp, in degrees, in scene model order. |
grasp_object_id | str | "" | Optional world object ID of the target. |
GraspData
Create it with grasps.GraspData() and set the fields you need.
| Field | Type | Default | Description |
|---|---|---|---|
tcp_frame | str | "" | Robot frame of the grasp TCP. Must exist in the model. |
ik_solver_frame | str | "" | Frame the IK solver targets. Must exist and share a parent joint with tcp_frame. |
attachment_frame | str | "" | Robot frame a grasped target object is attached to for the lift and retreat. Must exist for the full sequence. |
gripper_object_name | str | "" | Name of the gripper attached in the scene with attach_collision_object. Required. |
ee_link_names | list[str] | [] | End-effector links that may touch the target object during approach and closing. |
touch_links | list[str] | [] | Robot links allowed to touch the target once it is attached for the lift and retreat. |
open_gripper_configuration | np.ndarray | empty | Gripper joint configuration when open. Its length must match the gripper model. |
closed_gripper_configuration | np.ndarray | empty | Gripper joint configuration when closed. Required for closing and for the full sequence. |
gripper_closing_steps | int | 10 | Number of collision-checked intervals from open to closed. Must be at least 1. |
approach_direction_tcp | np.ndarray | [0, 0, 1] | Approach direction in the TCP frame. Must be nonzero. |
approach_distance_m | float | 0.0 | Approach distance in meters. Must be greater than 0. |
lift_distance_m | float | 0.0 | Lift distance in meters along the base Z axis. Must be greater than 0 for the full sequence. |
retreat_direction_tcp | np.ndarray | [0, 0, 0] | Retreat direction in the grasp TCP frame. Must be nonzero for the full sequence. |
retreat_distance_m | float | 0.0 | Retreat distance in meters. Must be greater than 0 for the full sequence. |
ik_settings | KinematicsSettings | default | Solver settings, normally robot.get_kinematics_settings(). |
max_step | MaxEEFStep | MaxEEFStep(0.01) | Largest Cartesian step between checked path samples. See Cartesian Interpolator. |
jump_threshold | JumpThreshold | JumpThreshold(4.0) | Largest allowed joint jump between samples. See Cartesian Interpolator. |
max_ik_seed_attempts | int | None | None | Maximum number of IK seeds the solver tries. None leaves the solver's own limit. If set, it must be at least 1. |
The approach path starts at the pregrasp joint solution that Grasp Filter found, so approach_direction_tcp and approach_distance_m are checked for validity but do not move the start. Set approach_distance_m to the same value as pre_grasp_offset_m, so the two agree.
Returns
Each planning method returns GraspPlanResult objects (plan_close returns one; the plan_all_* methods return a list).
| Field | Type | Description |
|---|---|---|
candidate_index | int | Position of the candidate in the input list. |
accepted | bool | Whether the candidate passed every check the method performs. |
rejection_reason | str | Empty when accepted. Otherwise one of the reasons below. |
approach_fraction | float | Fraction of the approach path completed, 0.0 to 1.0. |
lift_fraction | float | Fraction of the lift path completed, 0.0 to 1.0. |
retreat_fraction | float | Fraction of the retreat path completed, 0.0 to 1.0. |
approach_joint_trajectory_deg | list[np.ndarray] | Joint positions along the approach, in degrees. A rejected plan can hold a partial path. |
lift_joint_trajectory_deg | list[np.ndarray] | Joint positions along the lift, in degrees. |
retreat_joint_trajectory_deg | list[np.ndarray] | Joint positions along the retreat, in degrees. |
approach_collision_joint_positions_deg | np.ndarray | None | Joints at a colliding state on the approach, set by plan_all_approaches and plan_all_approach_and_close when the approach is rejected for a collision. Otherwise None. |
approach_colliding_geometry_names | list[str] | Geometry names in collision on the approach. |
gripper_closing_trajectory | list[np.ndarray] | Gripper configurations from open to closed, in native joint units. |
gripper_closing_fraction | float | Fraction of the closing motion completed without a collision, 0.0 to 1.0. |
closing_colliding_geometry_names | list[str] | Geometry names in collision while closing. |
object_attached | bool | Whether the target object was attached to the gripper for the lift and retreat. |
octomap_target_unresolved | bool | True when a named target exists in the world and the scene still has an OctoMap that may contain it. The plan is then not accepted. |
Rejection reasons
rejection_reason | Meaning |
|---|---|
"grasp_filter_rejected" | The filter status was not VALID, so the candidate was not planned. |
"collision" | The approach path stopped at a collision. |
"incomplete_cartesian_path" | A path ended before the target pose, or the lift and retreat were too incomplete. |
"target_occupancy_remains_in_octomap" | A named target is still part of the scene's OctoMap. Pass a scene that excludes it. |
"gripper_closing_collision" | The gripper collided while closing. |
Raises
| Exception | Condition |
|---|---|
TypeError | A scene argument is not a PlanningScene. |
ValueError | The candidates, filter results and object IDs do not align; a filter result's candidate_index does not match its position; a VALID filter result lacks its joint solutions; a frame is unknown or the gripper attachment is missing; a gripper configuration has the wrong size or is not finite; an approach, lift or retreat direction or distance is invalid; gripper_closing_steps or max_ik_seed_attempts is not positive; or a candidate pose is not a rigid transform. |
Method Reference
| Member | Description |
|---|---|
GraspPlanner() | Create a planner. |
plan_close(*, target_excluded_scene, grasp_data, grasp_joint_positions_deg, grasp_object_id="") | Check gripper closing with the arm fixed at the given joints. |
plan_all_approaches(*, grasp_candidates, grasp_filter_results, planning_scene, grasp_data, grasp_object_ids=None) | Check pregrasp-to-grasp paths. |
plan_all_approach_and_close(*, grasp_candidates, grasp_filter_results, planning_scene, target_excluded_scene, grasp_data, grasp_object_ids=None) | Check the approach, then the gripper closing at the grasp. |
plan_all_approach_lift_retreat(*, grasp_candidates, grasp_filter_results, planning_scene, grasp_data, grasp_object_ids=None) | Check the approach, lift and retreat paths. |
GraspData | Frames, gripper configurations and motion settings shared by a batch. |
GraspPlanResult | Read-only result for one candidate. |
Where to Use the Skill
- Confirming a grasp is executable - Keep only candidates whose approach, lift and retreat paths are free of collisions.
- Checking gripper closing - Make sure the fingers can close at the grasp without hitting the bin or a neighboring part.
- Driving motion commands - Use the returned joint trajectories as the basis for commands such as Set Joint Positions.
When Not to Use the Skill
- You only need to know whether a pose is reachable - use Grasp Filter, which is faster because it checks endpoints only.
- You need a path between arbitrary poses - use Cartesian Interpolator directly.
- You expect the robot or gripper to move - the planner only returns joint trajectories and gripper configurations. It never commands hardware or a simulator.