Skip to content

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 ​

python
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 ​

python
"""
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 ​

bash
python grasp_planner.py

To save the Rerun recording to a file instead of opening the viewer:

bash
python grasp_planner.py --save-rrd grasp_planner.rrd

For all options:

bash
python grasp_planner.py --help

What Each Method Checks ​

MethodChecksResult fields set
plan_all_approachesA 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_closeThe 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_closeOnly 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_retreatApproach 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 one GraspPlanResult per input, in input order, including rejected candidates. result.candidate_index equals the position in the input list.
  • grasp_candidates, grasp_filter_results and (if given) grasp_object_ids must have the same length. Each filter result's candidate_index must equal its position. Otherwise a ValueError is raised.
  • A candidate whose filter status is not VALID is not planned. Its result has accepted == False and rejection_reason == "grasp_filter_rejected".
  • A VALID filter result must hold both joint solutions, so run the filter with pre_grasp_offset_m, or the planner raises a ValueError.
  • Candidate poses must be valid rigid transforms. Poses stored as float32 can carry enough rotation error to be rejected with a ValueError; re-orthonormalize them first.

Parameter Configuration ​

plan_all_approaches, plan_all_approach_lift_retreat ​

All arguments are keyword-only.

ParameterTypeDefaultDescription
grasp_candidatesSequence[dict]requiredOne 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_resultsSequence[GraspFilterResult]requiredThe Grasp Filter results for the same batch, aligned by position.
planning_scenePlanningScenerequiredScene with the robot, the attached gripper and the obstacles.
grasp_dataGraspDatarequiredFrames, gripper configurations and motion settings. See below.
grasp_object_idsSequence[str] | NoneNoneOptional 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:

ParameterTypeDefaultDescription
target_excluded_scenePlanningScenerequiredA 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.

ParameterTypeDefaultDescription
target_excluded_scenePlanningScenerequiredScene whose OctoMap omits the target's occupancy.
grasp_dataGraspDatarequiredNeeds the gripper object, the open and closed configurations and gripper_closing_steps.
grasp_joint_positions_degSequence[float]requiredArm joint positions at the grasp, in degrees, in scene model order.
grasp_object_idstr""Optional world object ID of the target.

GraspData ​

Create it with grasps.GraspData() and set the fields you need.

FieldTypeDefaultDescription
tcp_framestr""Robot frame of the grasp TCP. Must exist in the model.
ik_solver_framestr""Frame the IK solver targets. Must exist and share a parent joint with tcp_frame.
attachment_framestr""Robot frame a grasped target object is attached to for the lift and retreat. Must exist for the full sequence.
gripper_object_namestr""Name of the gripper attached in the scene with attach_collision_object. Required.
ee_link_nameslist[str][]End-effector links that may touch the target object during approach and closing.
touch_linkslist[str][]Robot links allowed to touch the target once it is attached for the lift and retreat.
open_gripper_configurationnp.ndarrayemptyGripper joint configuration when open. Its length must match the gripper model.
closed_gripper_configurationnp.ndarrayemptyGripper joint configuration when closed. Required for closing and for the full sequence.
gripper_closing_stepsint10Number of collision-checked intervals from open to closed. Must be at least 1.
approach_direction_tcpnp.ndarray[0, 0, 1]Approach direction in the TCP frame. Must be nonzero.
approach_distance_mfloat0.0Approach distance in meters. Must be greater than 0.
lift_distance_mfloat0.0Lift distance in meters along the base Z axis. Must be greater than 0 for the full sequence.
retreat_direction_tcpnp.ndarray[0, 0, 0]Retreat direction in the grasp TCP frame. Must be nonzero for the full sequence.
retreat_distance_mfloat0.0Retreat distance in meters. Must be greater than 0 for the full sequence.
ik_settingsKinematicsSettingsdefaultSolver settings, normally robot.get_kinematics_settings().
max_stepMaxEEFStepMaxEEFStep(0.01)Largest Cartesian step between checked path samples. See Cartesian Interpolator.
jump_thresholdJumpThresholdJumpThreshold(4.0)Largest allowed joint jump between samples. See Cartesian Interpolator.
max_ik_seed_attemptsint | NoneNoneMaximum 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).

FieldTypeDescription
candidate_indexintPosition of the candidate in the input list.
acceptedboolWhether the candidate passed every check the method performs.
rejection_reasonstrEmpty when accepted. Otherwise one of the reasons below.
approach_fractionfloatFraction of the approach path completed, 0.0 to 1.0.
lift_fractionfloatFraction of the lift path completed, 0.0 to 1.0.
retreat_fractionfloatFraction of the retreat path completed, 0.0 to 1.0.
approach_joint_trajectory_deglist[np.ndarray]Joint positions along the approach, in degrees. A rejected plan can hold a partial path.
lift_joint_trajectory_deglist[np.ndarray]Joint positions along the lift, in degrees.
retreat_joint_trajectory_deglist[np.ndarray]Joint positions along the retreat, in degrees.
approach_collision_joint_positions_degnp.ndarray | NoneJoints 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_nameslist[str]Geometry names in collision on the approach.
gripper_closing_trajectorylist[np.ndarray]Gripper configurations from open to closed, in native joint units.
gripper_closing_fractionfloatFraction of the closing motion completed without a collision, 0.0 to 1.0.
closing_colliding_geometry_nameslist[str]Geometry names in collision while closing.
object_attachedboolWhether the target object was attached to the gripper for the lift and retreat.
octomap_target_unresolvedboolTrue 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_reasonMeaning
"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 ​

ExceptionCondition
TypeErrorA scene argument is not a PlanningScene.
ValueErrorThe 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 ​

MemberDescription
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.
GraspDataFrames, gripper configurations and motion settings shared by a batch.
GraspPlanResultRead-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.