The goal of this lab is to implement grasp planning in simulation for a known rigid object model and a simple gripper (e.g., a parallel-jaw hand). You will:

• Represent candidate grasps as contact sets on the object surface.
• Construct the grasp map and friction-cone approximation.
• Compute an analytic grasp quality score for each candidate.
• Validate top grasps in a physics engine by attempting pick-and-lift trials.

We assume the concepts of contact models, grasp wrench space (GWS) and quality measures from Chapter 7, and sampling-based motion planning from Chapters 1–3.

flowchart TD
  M["Mesh model (vertices, faces)"] --> SAMP["Sample surface points / pairs"]
  SAMP --> CAND["Construct candidate hand poses"]
  CAND --> ANAL["Build grasp map G, compute q(g)"]
  ANAL --> SEL["Select top K grasps"]
  SEL --> SIM["Simulate pick-and-lift trials"]
  SIM --> LOG["Record success rate & compare to q(g)"]
        

Consider a rigid object with body frame \( \mathcal{F}_O \). A grasp is defined by a set of \( m \) contacts, each with a contact frame \( \mathcal{F}_{C_i} \) attached at position \( \mathbf{p}_i \in \mathbb{R}^3 \) (expressed in \( \mathcal{F}_O \)) and outward unit normal \( \mathbf{n}_i \in \mathbb{R}^3 \).

A contact force at contact \( i \) is \( \mathbf{f}_i \in \mathbb{R}^3 \), decomposed into normal and tangential components with respect to \( \mathbf{n}_i \). The wrench (force+torque) on the object due to this contact is

\[ \mathbf{w}_i = \begin{bmatrix} \mathbf{f}_i \\ \mathbf{p}_i \times \mathbf{f}_i \end{bmatrix} \in \mathbb{R}^6. \]

Collecting all contacts, \( \mathbf{f} = [\mathbf{f}_1^\top,\dots,\mathbf{f}_m^\top]^\top \in \mathbb{R}^{3m} \), the total wrench is

\[ \mathbf{w} = \sum_{i=1}^m \mathbf{w}_i = \mathbf{G}\mathbf{f}, \quad \mathbf{G} = \begin{bmatrix} \mathbf{I}_3 & \cdots & \mathbf{I}_3 \\ \mathbf{p}_1^{\times} & \cdots & \mathbf{p}_m^{\times} \end{bmatrix} \in \mathbb{R}^{6 \times 3m}, \]

where \( \mathbf{p}_i^{\times} \) is the skew-symmetric matrix such that \( \mathbf{p}_i^{\times}\mathbf{f}_i = \mathbf{p}_i \times \mathbf{f}_i \).

For Coulomb friction with coefficient \( \mu_i \) at contact \( i \), the admissible contact forces form a second-order cone

\[ \mathcal{F}_i = \left\{ \mathbf{f}_i \in \mathbb{R}^3 \,\middle|\, f_{i,n} \ge 0,\; \sqrt{f_{i,t}^2 + f_{i,o}^2} \le \mu_i f_{i,n} \right\}. \]

In planning we approximate each cone by a polyhedral cone spanned by \( k \) edge directions \( \mathbf{b}_{i1},\dots,\mathbf{b}_{ik} \), e.g.

\[ \mathcal{F}_i \approx \left\{ \mathbf{f}_i \mid \mathbf{f}_i = \sum_{j=1}^k \alpha_{ij} \mathbf{b}_{ij}, \; \alpha_{ij} \ge 0 \right\}. \]

Each edge direction induces a primitive wrench \( \mathbf{w}_{ij} \) at contact \( i \), and the set of all primitive wrenches is \( \{\mathbf{w}_\ell\}_{\ell=1}^L \) with \(L = mk\). The grasp wrench space (GWS) is the convex cone generated by these primitive wrenches.

For a known triangular mesh model of the object, we can sample potential contact locations from its surface. Let the mesh have vertices \( V = \{\mathbf{v}_s\}_{s=1}^{N_V} \) and faces \( F = \{(s_1,s_2,s_3)\} \). We can:

1. Sample points uniformly over faces (e.g. via barycentric sampling).
2. Estimate surface normals at sample points (per-face or per-vertex).
3. Pair points whose normals are approximately opposite to obtain antipodal candidate grasps for a parallel-jaw gripper.

A simple antipodal condition for frictionless contacts is that the line segment between two points lies entirely inside the object and their normals are colinear and opposite. With friction, we allow a tolerance angle \( \theta_{\max} \) determined by \( \mu \).

\[ \angle(\mathbf{n}_1,-\mathbf{n}_2) \le \theta_{\max}, \quad \theta_{\max} = \arctan(\mu). \]

For each contact pair \( (\mathbf{p}_1,\mathbf{p}_2) \) satisfying this, we compute a gripper pose such that the finger pads lie at these positions and the closing direction is aligned with the connecting segment \( \mathbf{d} = \mathbf{p}_2 - \mathbf{p}_1 \).

flowchart TD
  A["Triangular mesh (V, F)"] --> B["Sample N surface points"]
  B --> C["Estimate normals at samples"]
  C --> D["Search pairs with nearly opposite normals"]
  D --> E["Check antipodal condition with mu"]
  E --> F["For each valid pair, compute gripper pose"]
        

Each candidate gripper pose induces contact positions \( \mathbf{p}_i \) and normals \( \mathbf{n}_i \), from which we construct the grasp map \( \mathbf{G} \) as in Section 2.

For each grasp candidate, we compute an analytic quality measure. A classical choice is the Ferrari–Canny epsilon metric, defined as the radius of the largest origin-centered ball contained in the convex hull of unit-magnitude primitive wrenches in the GWS. Let \( \mathcal{W} = \{\mathbf{w}_\ell\}_{\ell=1}^L \) denote these wrenches. Then

\[ \epsilon(\mathcal{G}) = \min_{\|\mathbf{u}\|_2 = 1} \max_{\ell \in \{1,\dots,L\}} \mathbf{u}^\top \mathbf{w}_\ell. \]

Intuitively, \( \epsilon(\mathcal{G}) \) measures the worst-case ability to resist disturbances in any direction. In implementation, we often approximate this using convex hull and minimum-distance computations, or via surrogates such as the smallest singular value of \( \mathbf{G} \).

We then validate a subset of grasps (e.g., the top \( K \) by \( \epsilon \)) in a physics engine (e.g., Bullet, ODE, MuJoCo):

1. Place the object on a virtual table in known pose.
2. Move the gripper to the candidate pose and close fingers.
3. Lift the object along world \( z \)-axis.
4. Mark the trial as success if the object remains grasped for a fixed time horizon.

This yields an empirical success probability \( \hat{p}_{\text{succ}}(g) \) for each grasp \( g \), which we can compare to \( \epsilon(g) \) to study how analytic metrics predict actual stability under dynamics.

flowchart TD
  S["Start"] --> L["Load object and gripper in simulator"]
  L --> G["Generate N candidates with quality q(g)"]
  G --> K["Select top K by q(g)"]
  K --> SIM["Run lift simulations for each grasp"]
  SIM --> R["Label success / failure, compute p_succ(g)"]
  R --> OUT["Analyze correlation between q(g) and p_succ(g)"]
        

We first implement the analytic side in Python using numpy and trimesh. For simplicity, we approximate the grasp quality by the smallest singular value of the grasp map, \( \sigma_{\min}(\mathbf{G}) \), which captures isotropy of the wrench generation.

import numpy as np
import trimesh

def skew(v):
    """Return skew-symmetric matrix for cross product."""
    x, y, z = v
    return np.array([[0.0, -z,   y],
                     [z,   0.0, -x],
                     [-y,  x,   0.0]])

def build_grasp_map(contact_points):
    """
    contact_points: list of 3D np.array, p_i in object frame.
    Parallel-jaw grasp with point contacts (for now no torsional friction).
    """
    m = len(contact_points)
    G_top = np.zeros((3, 3 * m))
    G_bottom = np.zeros((3, 3 * m))
    for i, p in enumerate(contact_points):
        idx = 3 * i
        G_top[:, idx:idx+3] = np.eye(3)
        G_bottom[:, idx:idx+3] = skew(p)
    G = np.vstack((G_top, G_bottom))
    return G

def epsilon_surrogate(G):
    """Use smallest singular value as surrogate quality."""
    s = np.linalg.svd(G, compute_uv=False)
    return float(np.min(s))

# Example: load a box mesh and define a simple two-finger grasp
mesh = trimesh.creation.box(extents=(0.1, 0.06, 0.04))

# Two opposing contacts on +y and -y faces
p1 = np.array([0.0,  0.03, 0.0])
p2 = np.array([0.0, -0.03, 0.0])

G = build_grasp_map([p1, p2])
q = epsilon_surrogate(G)
print("Quality surrogate sigma_min(G):", q)

def rank_and_filter_grasps(candidates, min_quality=1e-3):
    """
    candidates: list of lists of contact points
    Returns: list of (candidate, quality) sorted by quality descending.
    """
    scored = []
    for contacts in candidates:
        G = build_grasp_map(contacts)
        q = epsilon_surrogate(G)
        if q >= min_quality:
            scored.append((contacts, q))
    scored.sort(key=lambda x: x[1], reverse=True)
    return scored
      

This code omits explicit friction-cone approximation and assumes point-contact models; you can extend it by adding multiple wrench primitives per contact corresponding to friction-cone edges.

We now sketch a PyBullet-based loop that takes the top \( K \) grasps and empirically tests them via pick and lift. The details of robot control (trajectory generation) can rely on basic inverse kinematics introduced earlier in the program.

import pybullet as p
import pybullet_data
import time
import numpy as np

def setup_sim():
    physics_client = p.connect(p.DIRECT)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0, 0, -9.81)
    plane_id = p.loadURDF("plane.urdf")
    return physics_client, plane_id

def load_scene_box():
    """Load a box as a URDF; use your own URDF or generated mesh."""
    box_id = p.loadURDF("cube_small.urdf", [0, 0, 0.02])
    return box_id

def load_gripper():
    # Replace with your gripper URDF (parallel-jaw)
    gripper_id = p.loadURDF("franka_panda/panda.urdf", [0, 0, 0])
    return gripper_id

def attempt_grasp(gripper_id, box_id, grasp_pose_world, close_joints, lift_height=0.15, steps=240):
    """
    grasp_pose_world: (pos, orn) of gripper frame in world coordinates.
    close_joints: list of joint positions for grasp closure.
    Returns True if object lifted and held.
    """
    pos, orn = grasp_pose_world
    # Teleport end-effector near grasp pose (for simplicity)
    # In practice, solve IK instead.
    ee_link = 11  # e.g. Franka Panda link index
    p.resetBasePositionAndOrientation(gripper_id, [0, 0, 0], [0, 0, 0, 1])
    # Here you would compute joint positions via IK solver

    # Close fingers
    for j, q in close_joints:
        p.setJointMotorControl2(gripper_id, j,
                                controlMode=p.POSITION_CONTROL,
                                targetPosition=q, force=50.0)

    for _ in range(steps):
        p.stepSimulation()

    # Lift motion (again, simplified as base motion or EE joint update)
    box_pos, _ = p.getBasePositionAndOrientation(box_id)
    start_z = box_pos[2]
    for _ in range(steps):
        # here move EE upwards using joint targets
        p.stepSimulation()

    box_pos, _ = p.getBasePositionAndOrientation(box_id)
    lifted = box_pos[2] > start_z + 0.05
    return lifted

def evaluate_grasps_in_sim(candidates, K=10):
    physics_client, plane_id = setup_sim()
    box_id = load_scene_box()
    gripper_id = load_gripper()

    # Suppose candidates is list of (grasp_pose_world, quality)
    topK = sorted(candidates, key=lambda x: x[1], reverse=True)[:K]
    results = []
    for grasp_pose_world, q in topK:
        success = attempt_grasp(gripper_id, box_id, grasp_pose_world,
                                close_joints=[(9, 0.04), (10, 0.04)])
        results.append((grasp_pose_world, q, success))
    p.disconnect(physics_client)
    return results
      

This loop is intentionally simplified: in a full implementation you would integrate a motion planner to bring the gripper to the candidate pose without collision, and use analytical IK rather than teleporting.

In C++, Eigen is a natural choice for linear algebra and Bullet or FCL for collision and physics. Below is a minimal snippet for constructing the grasp map and computing the smallest singular value.

#include <Eigen/Dense>
#include <vector>
#include <iostream>

using Vec3 = Eigen::Vector3d;
using Mat  = Eigen::MatrixXd;

Eigen::Matrix3d skew(const Vec3 &v) {
    Eigen::Matrix3d S;
    S <<  0.0,   -v.z(),  v.y(),
            v.z(),  0.0,   -v.x(),
           -v.y(),  v.x(),  0.0;
    return S;
}

Mat buildGraspMap(const std::vector<Vec3> &contacts) {
    const std::size_t m = contacts.size();
    Mat G_top(3, 3 * m);
    Mat G_bottom(3, 3 * m);
    G_top.setZero();
    G_bottom.setZero();

    for (std::size_t i = 0; i < m; ++i) {
        std::size_t idx = 3 * i;
        G_top.block<3,3>(0, idx) = Eigen::Matrix3d::Identity();
        G_bottom.block<3,3>(0, idx) = skew(contacts[i]);
    }
    Mat G(6, 3 * m);
    G << G_top,
         G_bottom;
    return G;
}

double epsilonSurrogate(const Mat &G) {
    Eigen::JacobiSVD<Mat> svd(G, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXd s = svd.singularValues();
    return s.minCoeff();
}

int main() {
    std::vector<Vec3> contacts;
    contacts.emplace_back(0.0,  0.03, 0.0);
    contacts.emplace_back(0.0, -0.03, 0.0);

    Mat G = buildGraspMap(contacts);
    double q = epsilonSurrogate(G);
    std::cout << "sigma_min(G) = " << q << std::endl;
    return 0;
}
      

Integrating with a physics engine (e.g., Bullet) involves creating a rigid-body world, loading the object and gripper, and applying joint torques or position controls to execute each grasp as in the Python example.

In Java, you can combine a linear algebra library (e.g., EJML) with a physics engine such as jBullet. The code below uses EJML-style dense matrices to construct a grasp map for two contacts.

import org.ejml.simple.SimpleMatrix;
import java.util.ArrayList;
import java.util.List;

class Vec3 {
    public double x, y, z;
    public Vec3(double x, double y, double z) {
        this.x = x; this.y = y; this.z = z;
    }
}

public class GraspQuality {

    public static SimpleMatrix skew(Vec3 v) {
        double[][] data = {
                { 0.0,   -v.z,  v.y },
                { v.z,    0.0, -v.x },
                { -v.y,   v.x,  0.0 }
        };
        return new SimpleMatrix(data);
    }

    public static SimpleMatrix buildGraspMap(List<Vec3> contacts) {
        int m = contacts.size();
        SimpleMatrix G_top = new SimpleMatrix(3, 3 * m);
        SimpleMatrix G_bottom = new SimpleMatrix(3, 3 * m);
        G_top.zero(); G_bottom.zero();

        for (int i = 0; i < m; ++i) {
            int idx = 3 * i;
            // Set identity block
            for (int r = 0; r < 3; ++r) {
                G_top.set(r, idx + r, 1.0);
            }
            SimpleMatrix S = skew(contacts.get(i));
            G_bottom.insertIntoThis(0, idx, S);
        }
        // Stack vertically
        SimpleMatrix G = new SimpleMatrix(6, 3 * m);
        G.insertIntoThis(0, 0, G_top);
        G.insertIntoThis(3, 0, G_bottom);
        return G;
    }

    public static double epsilonSurrogate(SimpleMatrix G) {
        SimpleMatrix[] svd = G.svd();
        SimpleMatrix S = svd[1]; // diagonal singular values
        double minSigma = Double.POSITIVE_INFINITY;
        int n = Math.min(S.numRows(), S.numCols());
        for (int i = 0; i < n; ++i) {
            double sigma = S.get(i, i);
            if (sigma < minSigma) {
                minSigma = sigma;
            }
        }
        return minSigma;
    }

    public static void main(String[] args) {
        List<Vec3> contacts = new ArrayList<>();
        contacts.add(new Vec3(0.0,  0.03, 0.0));
        contacts.add(new Vec3(0.0, -0.03, 0.0));

        SimpleMatrix G = buildGraspMap(contacts);
        double q = epsilonSurrogate(G);
        System.out.println("sigma_min(G) = " + q);
    }
}
      

The physics loop in Java mirrors the Python PyBullet example but uses the chosen Java physics engine to execute and evaluate grasp trials.

MATLAB with Robotics System Toolbox and Simscape Multibody is well suited for combining analytic grasp quality with multibody simulation. The following code builds a simple grasp map and computes the smallest singular value as a surrogate quality.

function q = grasp_quality_example()
    % Two contact points on a box
    p1 = [0.0;  0.03; 0.0];
    p2 = [0.0; -0.03; 0.0];

    G = buildGraspMap({p1, p2});
    s = svd(G);
    q = min(s);
    fprintf('sigma_min(G) = %f\n', q);
end

function S = skew(v)
    x = v(1); y = v(2); z = v(3);
    S = [ 0, -z,  y;
          z,  0, -x;
         -y,  x,  0];
end

function G = buildGraspMap(contacts)
    m = numel(contacts);
    G_top = zeros(3, 3*m);
    G_bottom = zeros(3, 3*m);
    for i = 1:m
        idx = 3*(i-1) + 1;
        G_top(:, idx:idx+2) = eye(3);
        G_bottom(:, idx:idx+2) = skew(contacts{i});
    end
    G = [G_top; G_bottom];
end
      

In Simulink, you can build a model where the gripper is commanded to a sequence of joint angles corresponding to different candidate grasps. Using Simscape contact blocks, you can check whether the object is stably lifted, and log success flags that can be compared to the analytic quality scores.

Mathematica provides convenient symbolic and numeric tools for grasp quality analysis. Below is a small script that constructs a planar grasp map and computes the minimum singular value.

(* Planar 3-DOF wrench: [fx, fy, tau]^T, two contact points *)
skew2D[v_] := { {0, -1}, {1, 0} } . v;

buildGraspMap2D[points_List] := Module[
  {m = Length[points], Gtop, Gbot, G},
  Gtop = ConstantArray[0.0, {2, 2 m}];
  Gbot = ConstantArray[0.0, {1, 2 m}];
  Do[
    With[{idx = 2 (i - 1) + 1, p = points[[i]]},
      Gtop[[1, idx]] = 1.0; Gtop[[2, idx + 1]] = 1.0;
      (* torque = p x f in 2D *)
      Gbot[[1, idx ;; idx + 1]] = skew2D[p],
      ];
    , {i, 1, m}];
  G = Join[Gtop, Gbot];
  G
];

p1 = {0.03, 0.0};
p2 = {-0.03, 0.0};
G = buildGraspMap2D[{p1, p2}];

svals = SingularValueList[N[G]];
q = Min[svals];
Print["sigma_min(G) = ", q];
      

You can extend this to 3D, visualize the grasp wrench space using ConvexHullMesh, and compute the distance from the origin to the hull as an approximation of the epsilon metric.

Problem 1 (Planar Force-Closure Test): Consider a planar object with two frictionless point contacts at positions \( \mathbf{p}_1 = (a,0)^\top \) and \( \mathbf{p}_2 = (-a,0)^\top \), with contact normals pointing horizontally inward. In 2D, the wrench is \( \mathbf{w} = (f_x, f_y, \tau)^\top \). Show that this grasp cannot be force-closure in the frictionless case.

Solution: For frictionless contacts, each force is colinear with the contact normal. The normals are horizontal, so both forces are along the \( x \)-axis: \( \mathbf{f}_1 = (\lambda_1, 0)^\top \) and \( \mathbf{f}_2 = (-\lambda_2, 0)^\top \) with \( \lambda_1,\lambda_2 \ge 0 \) (compressive only). The total force is

\[ \mathbf{f} = \begin{bmatrix} \lambda_1 - \lambda_2 \\ 0 \end{bmatrix}. \]

The net torque about the origin is \( \tau = \mathbf{p}_1 \times \mathbf{f}_1 + \mathbf{p}_2 \times \mathbf{f}_2 \), which is zero because each force acts through the line of centers with zero moment arm relative to the center: \( \tau = 0 \). Thus the wrench set contains only horizontal forces with zero torque and cannot balance arbitrary external wrenches (e.g., a vertical force). Hence the grasp is not force-closure.

Problem 2 (Friction Cone Linearization): For a 3D contact with friction coefficient \( \mu \), we approximate the friction cone by a pyramid with \( k \) edges. Let \( \mathbf{n} \) be the normal and \( \mathbf{t}_1,\mathbf{t}_2 \) be orthonormal tangential directions. Construct expressions for the edge directions \( \mathbf{b}_j \) in terms of \( \mathbf{n},\mathbf{t}_1,\mathbf{t}_2,\mu \).

Solution: One common construction is to place the edge directions uniformly on a circle in the tangential plane at the boundary of the friction cone. Let \( \theta_j = 2\pi j/k \) for \( j = 0,\dots,k-1 \). Define tangential unit vectors

\[ \mathbf{t}(\theta_j) = \cos\theta_j\,\mathbf{t}_1 + \sin\theta_j\,\mathbf{t}_2. \]

The boundary of the cone satisfies \( \|\mathbf{f}_t\|_2 = \mu f_n \). A unit edge direction on this boundary can be taken as

\[ \mathbf{b}_j = \frac{1}{\sqrt{1+\mu^2}} \left( \mu\,\mathbf{t}(\theta_j) + \mathbf{n} \right). \]

These \( \mathbf{b}_j \) span a polyhedral cone that approximates the true friction cone; increasing \( k \) improves the approximation.

Problem 3 (Singular Value Surrogate and Isotropy): Let \( \mathbf{G} \in \mathbb{R}^{6 \times 3m} \) be the grasp map for a given grasp, and suppose \( \mathbf{G} \) has singular values \( \sigma_1 \ge \dots \ge \sigma_6 > 0 \). Show that the induced operator norm satisfies \( \|\mathbf{G}\|_2 = \sigma_1 \) and that the smallest singular value \( \sigma_6 \) gives a lower bound on the wrench magnitude achievable by unit-norm contact forces.

Solution: By definition of the spectral norm,

\[ \|\mathbf{G}\|_2 = \max_{\|\mathbf{f}\|_2 = 1} \|\mathbf{G}\mathbf{f}\|_2 = \sigma_1. \]

Similarly, the smallest singular value satisfies

\[ \sigma_6 = \min_{\|\mathbf{f}\|_2 = 1} \|\mathbf{G}\mathbf{f}\|_2. \]

Thus for any unit contact-force vector \( \mathbf{f} \) we have \( \|\mathbf{G}\mathbf{f}\|_2 \ge \sigma_6 \). This means no matter how we choose unit-norm contact forces, the resulting wrench magnitude is at least \( \sigma_6 \), so \( \sigma_6 \) measures the worst-case ability to generate wrenches and is a reasonable surrogate for isotropic grasp quality.

Problem 4 (Simulation-Based Success Probability): Suppose for a given grasp \( g \) you run \( N \) independent simulations with randomized small perturbations in object pose and friction coefficient, and observe \( S \) successful lifts. Assuming a Bernoulli model, derive an approximate \( (1-\alpha) \) confidence interval for the true success probability \( p_{\text{succ}}(g) \).

Solution: Let \( \hat{p} = S/N \). For moderately large \( N \), the normal approximation yields

\[ \hat{p} \approx \mathcal{N} \left( p_{\text{succ}}, \frac{p_{\text{succ}}(1-p_{\text{succ}})}{N} \right). \]

Replacing \( p_{\text{succ}} \) by \( \hat{p} \) in the variance gives an approximate \( (1-\alpha) \) confidence interval

\[ \hat{p} \pm z_{1-\alpha/2} \sqrt{\frac{\hat{p}(1-\hat{p})}{N}}, \]

where \( z_{1-\alpha/2} \) is the corresponding standard normal quantile (e.g., \( z_{0.975} \approx 1.96 \) for a 95% interval).

In this lab you implemented the complete pipeline from object mesh and contact modeling to analytic grasp quality and simulation-based evaluation. You constructed grasp maps, approximated friction cones, and used singular-value-based quality surrogates. You then used physics simulation to empirically estimate grasp success probability and relate it to analytic measures. These skills form the basis for more advanced grasp synthesis and dexterous manipulation planning in subsequent chapters, including integration with task-level planning and learning components.