From Object Poses to Grasp Targets

Knowing a mug's 6D pose is only the beginning of manipulation. To grasp it, a robot must generate candidate gripper poses, evaluate collision-free approach vectors, check finger stroke clearance, and choose contact patches that satisfy force closure.

Grasp target planning generates pre-grasp standoffs and checks friction cones for antipodal force closure

Why Does This Exist?

In computer vision pipelines, the output of a 3D perception model is a 6D rigid body pose: an object center $[x, y, z]$ and a $3 \times 3$ rotation matrix. But a robot cannot simply move its fingers to that coordinate: the center of a coffee mug is solid ceramic, and plunging the gripper directly into the object shatters the cup or trips a motor torque limit.

Converting an estimated object pose into an actionable grasp target requires solving a physical contact problem:

Approach vector: Where does the gripper approach from so its fingers do not collide with the table or neighboring obstacles?
Pre-grasp standoff: An intermediate waypoint where fingers open fully before the final straight-line approach.
Contact mechanics: Will the contact forces resist gravity, slip, and inertia when the arm accelerates?

Without this bridge, perception produces accurate object detections while physical manipulation fails completely.

Basics first: estimating 6D object poses from visual pixels lives in 6d-pose-estimation. Transforming coordinates across frames lives in robot-coordinate-frames. Solving the arm joint angles that achieve the planned grasp pose lives in robot-forward-inverse-kinematics.

Think of It Like This

Picking up a slippery glass jar from a crowded pantry shelf

Imagine reaching into a narrow pantry shelf to lift a smooth glass jam jar. Your eyes pinpoint the jar's location immediately. But before your fingers close, you execute a subconscious sequence:

You do not reach straight down from above, because the shelf above blocks your hand. Instead, you approach horizontally from the front. You hold your fingers open wide at a standoff distance five centimeters in front of the jar, verify your knuckles will not hit the peanut butter jar beside it, slide your fingertips smoothly around the glass, and pinch with opposing thumb and fingers.

Where it stops: human skin is compliant, soft, and moist, providing high friction and immediate tactile nerve feedback. If the jar starts to slip, you instinctively tighten your grip in milliseconds. A standard parallel-jaw robotic gripper has rigid or semi-compliant silicone pads: if its contact angles fall outside the friction cone, the jar squirts out like a wet watermelon seed.

How It Actually Works

The grasp planning pipeline

Grasp planning translates an object pose $\mathbf{T}_{W \leftarrow O}$ into an end-effector trajectory through four stages:

Grasp Candidate Generation: Samples candidate grasp poses $\mathbf{T}_{O \leftarrow G}$ relative to the object's geometry. For a cylindrical mug, grasps sample radially around the cylinder wall or along the handle.
Standoff / Pre-Grasp Pose: Backs the gripper off along the approach unit vector $\hat{\mathbf{a}}$ by a standoff distance $d_{\text{standoff}}$ (typically $5\text{ cm}$ to $15\text{ cm}$ ): $\mathbf{p}_{\text{pre}} = \mathbf{p}_{\text{grasp}} - d_{\text{standoff}} \hat{\mathbf{a}}$
Finger Clearance Check: Verifies that the gripper's maximum jaw stroke $w_{\text{max}}$ exceeds the object thickness $w_{\text{obj}}$ by a safety margin: $w_{\text{max}} \ge w_{\text{obj}} + 2 \times \text{margin}$
Force Closure and Friction Cones: Ensures contact forces can resist external wrenches without slipping.

Antipodal grasps and friction cones

For a two-finger parallel gripper, an antipodal grasp places two contact points $\mathbf{c}_1, \mathbf{c}_2$ on opposing sides of the object. According to Coulomb's friction model, contact force $\mathbf{F}$ must lie inside a friction cone centered on the inward surface normal $\hat{\mathbf{n}}$ :

\|\mathbf{F}_t\| \le \mu F_n

where $\mu$ is the static friction coefficient, $\mathbf{F}_t$ is the tangential shear force, and $F_n$ is the normal gripping force.

The half-angle $\phi$ of the friction cone is:

\phi = \arctan(\mu)

Force closure requires the line connecting contact points $\mathbf{c}_1$ and $\mathbf{c}_2$ to fall inside both friction cones. If the surface normal angle deviation $\theta_{\text{normal}}$ exceeds $\phi$ , the fingers slide off the surface upon closing.

Worked example: deriving pre-grasp and grasp targets for a mug

A vision system estimates a cylindrical coffee mug pose on a tabletop:

Object position in robot base frame: $\mathbf{p}_O = [0.45, 0.20, 0.10]^T\text{ m}$ .
Mug diameter: $w_{\text{obj}} = 0.08\text{ m}$ ( $8\text{ cm}$ ).
Gripper maximum opening stroke: $w_{\text{max}} = 0.12\text{ m}$ ( $12\text{ cm}$ ).
Approach direction: horizontal along the base $x$ -axis, $\hat{\mathbf{a}} = [1.0, 0.0, 0.0]^T$ .
Standoff distance: $d_{\text{standoff}} = 0.10\text{ m}$ ( $10\text{ cm}$ ).
Surface friction coefficient: $\mu = 0.50$ .

First, verify finger stroke clearance:

\text{Clearance} = \frac{w_{\text{max}} - w_{\text{obj}}}{2} = \frac{0.12 - 0.08}{2} = \frac{0.04}{2} = 0.02\text{ m} = 2.0\text{ cm per finger}

The $2\text{ cm}$ clearance per side provides sufficient margin against visual pose estimation uncertainty.

Second, compute the pre-grasp waypoint $\mathbf{p}_{\text{pre}}$ :

\mathbf{p}_{\text{pre}} = \mathbf{p}_O - d_{\text{standoff}} \hat{\mathbf{a}} = [0.45, 0.20, 0.10]^T - 0.10 [1.0, 0.0, 0.0]^T = [0.35, 0.20, 0.10]^T\text{ m}

Third, compute the friction cone half-angle $\phi$ :

\phi = \arctan(0.50) \approx 0.4636\text{ rad} \approx 26.57^\circ

The parallel gripper contacts the cylinder with an angle deviation $\theta = 12.0^\circ$ relative to the surface normal. Because $12.0^\circ < 26.57^\circ$ , the contact forces lie safely within the friction cone, satisfying antipodal force closure.

The execution sequence is:

Open gripper to $12\text{ cm}$ .
Move arm through collision-free space to pre-grasp pose $\mathbf{p}_{\text{pre}} = [0.35, 0.20, 0.10]^T\text{ m}$ .
Linear Cartesian descent along $\hat{\mathbf{a}}$ to $\mathbf{p}_O = [0.45, 0.20, 0.10]^T\text{ m}$ .
Close fingers until torque/current threshold indicates firm contact.

Code

This Python snippet computes pre-grasp waypoints from estimated object poses, verifies gripper stroke clearance, and checks the Coulomb friction cone condition for grasp stability.

1import math2import numpy as np3
4
5def plan_grasp_target(6    p_obj: np.ndarray,7    v_approach: np.ndarray,8    standoff_m: float,9    obj_width_m: float,10    gripper_max_stroke_m: float,11    contact_angle_deg: float,12    friction_coeff: float = 0.50,13) -> tuple[list[float], bool, bool]:14    """Compute pre-grasp pose and verify stroke clearance and friction cone."""15    # Pre-grasp position backed off along approach vector16    v_norm: np.ndarray = v_approach / np.linalg.norm(v_approach)17    p_pre: np.ndarray = p_obj - standoff_m * v_norm18
19    # Clearance check20    has_clearance: bool = gripper_max_stroke_m >= (obj_width_m + 0.02)21
22    # Friction cone check23    cone_limit_deg: float = math.degrees(math.atan(friction_coeff))24    is_force_closure: bool = contact_angle_deg <= cone_limit_deg25
26    return [round(float(x), 3) for x in p_pre], has_clearance, is_force_closure27
28
29p_pre_target, clearance_ok, stable_ok = plan_grasp_target(30    p_obj=np.array([0.45, 0.20, 0.10]),31    v_approach=np.array([1.0, 0.0, 0.0]),32    standoff_m=0.10,33    obj_width_m=0.08,34    gripper_max_stroke_m=0.12,35    contact_angle_deg=12.0,36    friction_coeff=0.50,37)38
39print(p_pre_target)  # -> [0.35, 0.2, 0.1]40print(clearance_ok)  # -> True41print(stable_ok)     # -> True

Watch Out For

Planning directly to the contact pose without a pre-grasp standoff

Sending a robot arm directly from an arbitrary joint configuration to the final contact grasp pose causes the gripper fingers to sweep through the object or scrape the tabletop during the final approach.

The symptom is the robot knocking the mug off the table before the fingers can close. Fix it by strictly enforcing two-phase trajectory generation: plan global motion to a collision-free pre-grasp standoff waypoint ( $5$ to $10\text{ cm}$ away), then execute a slow, linear Cartesian guarded approach along the tool approach vector.

Assuming a valid grasp candidate is kinematically reachable

Generating a high-scoring grasp pose on a mug does not guarantee the robot arm can physically reach it: the required wrist orientation might violate joint limits, collide with the robot's own torso, or pass through a kinematic singularity.

The symptom is the grasp planner outputting perfect targets, but the execution stack failing with "No Inverse Kinematics Solution." Fix it by filtering candidate grasp poses through an inverse kinematics and collision-checking query before ranking them for execution.

The Quick Version

Turning a 6D object pose into a successful grasp requires approach vectors, pre-grasp standoffs, finger clearance, and contact force closure.
A pre-grasp standoff backs the open gripper off along the approach vector (e.g. at $[0.35, 0.20, 0.10]\text{ m}$ for a mug at $[0.45, 0.20, 0.10]\text{ m}$ ) to prevent approach collisions.
Gripper stroke must exceed object width with sufficient margin to tolerate perception errors ( $2\text{ cm}$ per finger).
Coulomb friction cones define contact stability: with $\mu = 0.50$ , contact angle deviations below $\arctan(0.50) \approx 26.57^\circ$ satisfy force closure without slipping.
Grasp candidate generation must be validated against arm inverse kinematics and table collisions before commanding execution.