Consider a rigid object with body frame \( \mathcal{F}_o \). A grasp configuration \( g \) induces \( m \) contacts at positions \( \mathbf{p}_i \in \mathbb{R}^3 \), unit outward normals \( \mathbf{n}_i \), and friction coefficient \( \mu_i \), for \( i = 1,\dots,m \).

Under a point-contact-with-friction model, the admissible contact forces at contact \( i \) form a friction cone

\[ \mathcal{C}_i = \left\{ \mathbf{f}_i \in \mathbb{R}^3 \;\middle|\; \mathbf{f}_i^\top \mathbf{n}_i \ge 0,\; \left\|\mathbf{f}_{i,t}\right\|_2 \le \mu_i\,\mathbf{f}_{i,n} \right\}, \]

where \( \mathbf{f}_{i,n} = (\mathbf{f}_i^\top \mathbf{n}_i)\mathbf{n}_i \) is the normal component and \( \mathbf{f}_{i,t} = \mathbf{f}_i - \mathbf{f}_{i,n} \) is the tangential component.

Each contact force induces a spatial wrench (force + moment) at the object frame origin:

\[ \mathbf{w}_i = \begin{bmatrix} \mathbf{f}_i \\ (\mathbf{p}_i - \mathbf{p}_o) \times \mathbf{f}_i \end{bmatrix} \in \mathbb{R}^6, \]

and, stacking all contacts, the total wrench is \( \mathbf{w} = \sum_{i=1}^m \mathbf{w}_i \). The set of all wrenches that the grasp can generate, subject to friction cones and unilateral contact, is the grasp wrench space (GWS) \( \mathcal{W}_g \subset \mathbb{R}^6 \).

Our goal in this lab is: given a discrete approximation \( \hat{\mathcal{W}}_g \) of the GWS for each candidate grasp \( g \), compute a scalar grasp score \( Q(g) \) that reflects robustness to external disturbances.

Let \( \mathcal{B} = \{\mathbf{w} \in \mathbb{R}^6 \mid \|\mathbf{w}\|_2 \le 1\} \) denote the unit ball of possible normalized external wrenches (e.g., gravity, pushes, inertial loads). The Ferrari–Canny \( \epsilon \)-quality of a grasp \( g \) is defined as

\[ Q_\epsilon(g) = \max \left\{ r \ge 0 \;\middle|\; r\,\mathcal{B} \subseteq \operatorname{conv}(\mathcal{W}_g) \right\}, \]

i.e., the radius of the largest Euclidean ball centered at the origin that fits inside the convex hull of the GWS. Geometrically, this is the worst-case margin against an arbitrary unit external wrench: larger \( Q_\epsilon \) implies more robustness.

Exact computation requires constructing a convex polytope in six dimensions. In practice we:

1. Approximate each friction cone \( \mathcal{C}_i \) by \( k \) generating rays \( \mathbf{f}_{i\ell}, \; \ell = 1,\dots,k \).
2. Map each ray to a wrench \( \mathbf{w}_{i\ell} \in \mathbb{R}^6 \).
3. Define the discrete wrench set \( \mathcal{S}_g = \{\mathbf{w}_{i\ell}\}_{i,\ell} \).
4. Compute an approximation of \( Q_\epsilon(g) \) from \( \mathcal{S}_g \).

A convenient sampling-based approximation is

\[ \hat{Q}_\epsilon(g) = \min_{j=1,\dots,N_d} \;\max_{\mathbf{w} \in \mathcal{S}_g} \mathbf{u}_j^\top \mathbf{w}, \]

where \( \mathbf{u}_j \) are random unit directions on \( \mathbb{S}^5 \). For each direction \( \mathbf{u}_j \), we compute the support function of \( \operatorname{conv}(\mathcal{S}_g) \). The minimum over directions approximates the distance from the origin to the closest supporting hyperplane, which equals \( Q_\epsilon(g) \).

The following flow describes the software pipeline used in all language implementations in this lab.

flowchart TD
  A["Input object mesh + candidate grasps"] --> B["For each grasp: compute contact points and normals"]
  B --> C["For each contact: discretize friction cone into k rays"]
  C --> D["Map each ray to 6D wrench (force + moment)"]
  D --> E["Aggregate all wrenches S_g"]
  E --> F["Sample Nd directions u_j on unit sphere in R^6"]
  F --> G["For each u_j compute support value max_w in S_g (u_j^T w)"]
  G --> H["Grasp score Q_hat_epsilon = min_j support_j"]
  H --> I["Rank grasps and select top N for execution"]
        

Note that collision checking and kinematic feasibility checks for candidate grasps are assumed to be handled upstream by a planner (from earlier chapters); this lab focuses exclusively on wrench-based scoring.

We first implement the grasp scoring core in Python using numpy. Optional geometry processing (e.g., computing contact points from meshes) can be done with libraries such as trimesh or open3d, but here we assume contact positions and normals are already given.

import numpy as np

def discretize_friction_cone(normal, mu, k=8):
    """
    Approximate a 3D Coulomb friction cone by k unit force directions.
    normal : (3,) array (unit vector)
    mu     : scalar friction coefficient
    """
    n = normal / np.linalg.norm(normal)
    # Build orthonormal basis {t1, t2, n}
    # Pick arbitrary vector not parallel to n
    tmp = np.array([1.0, 0.0, 0.0])
    if abs(np.dot(tmp, n)) > 0.9:
        tmp = np.array([0.0, 1.0, 0.0])
    t1 = np.cross(n, tmp)
    t1 /= np.linalg.norm(t1)
    t2 = np.cross(n, t1)

    dirs = []
    angles = np.linspace(0.0, 2.0 * np.pi, k, endpoint=False)
    for theta in angles:
        # Tangential direction in the friction circle
        t = np.cos(theta) * t1 + np.sin(theta) * t2
        # Construct a direction on the cone boundary
        # f = n + mu * t, then normalize
        f = n + mu * t
        f /= np.linalg.norm(f)
        dirs.append(f)
    return np.stack(dirs, axis=0)  # shape (k, 3)


def contact_wrenches(p, n, mu, k=8, wrench_scale=1.0):
    """
    Generate 6D wrenches for a single contact.
    p : (3,) contact position in object frame
    n : (3,) outward normal (unit vector)
    mu: friction coefficient
    Returns: array (k, 6)
    """
    ray_dirs = discretize_friction_cone(n, mu, k)
    wrenches = []
    for f_dir in ray_dirs:
        # Scale to max normal force = wrench_scale
        # Here we just use wrench_scale as a normalization constant.
        f = wrench_scale * f_dir
        m = np.cross(p, f)  # moment at origin
        w = np.concatenate([f, m])
        wrenches.append(w)
    return np.stack(wrenches, axis=0)


def build_wrench_set(contacts, k=8, mu_default=0.8):
    """
    contacts: list of dicts with keys:
      'p': np.array(3,), 'n': np.array(3,), optional 'mu'
    Returns: array (M, 6) of all contact wrenches.
    """
    all_w = []
    for c in contacts:
        p = c["p"]
        n = c["n"]
        mu = c.get("mu", mu_default)
        Wc = contact_wrenches(p, n, mu, k=k)
        all_w.append(Wc)
    return np.vstack(all_w)


def sample_unit_directions(dim, num_samples):
    """
    Sample random unit vectors in R^dim using Gaussian normalization.
    """
    X = np.random.normal(size=(num_samples, dim))
    X /= np.linalg.norm(X, axis=1, keepdims=True)
    return X  # shape (num_samples, dim)


def epsilon_quality(wrenches, num_directions=256):
    """
    Approximate Ferrari-Canny epsilon quality.
    wrenches: array (M, 6)
    """
    if wrenches.shape[0] == 0:
        return 0.0
    dirs = sample_unit_directions(6, num_directions)  # u_j
    # Compute support values: for each direction, max(u_j^T w)
    # wrenches.T has shape (6, M)
    proj = dirs.dot(wrenches.T)  # shape (num_directions, M)
    support_vals = np.max(proj, axis=1)  # max over w
    # Epsilon approx is min over directions
    return float(np.min(support_vals))


def score_grasp(contacts, k=8, mu_default=0.8, num_directions=256):
    """
    High-level: contacts -> epsilon grasp score.
    """
    W = build_wrench_set(contacts, k=k, mu_default=mu_default)
    return epsilon_quality(W, num_directions=num_directions)


if __name__ == "__main__":
    # Example: simple planar-ish three-finger grasp around the origin
    contacts = [
        {"p": np.array([0.05, 0.0, 0.0]), "n": np.array([-1.0, 0.0, 0.0]), "mu": 0.7},
        {"p": np.array([-0.05, 0.04, 0.0]), "n": np.array([1.0, -1.0, 0.0]), "mu": 0.7},
        {"p": np.array([-0.05, -0.04, 0.0]), "n": np.array([1.0, 1.0, 0.0]), "mu": 0.7},
    ]
    # Normalize normals
    for c in contacts:
        c["n"] = c["n"] / np.linalg.norm(c["n"])

    q = score_grasp(contacts, k=8, mu_default=0.7, num_directions=512)
    print("Approximate epsilon quality:", q)
      

This Python code directly mirrors the theoretical pipeline: friction cone discretization, wrench generation, and support-function based approximation of \( Q_\epsilon \).

In C++, linear algebra is conveniently handled using Eigen. The following snippet shows the core routines for computing the epsilon quality of a grasp, assuming we already have a vector of contacts.

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

struct Contact {
  Eigen::Vector3d p;   // position
  Eigen::Vector3d n;   // unit normal
  double mu;           // friction coefficient
};

std::vector<Eigen::Vector3d>
discretizeFrictionCone(const Eigen::Vector3d& n, double mu, int k) {
  Eigen::Vector3d nn = n.normalized();
  Eigen::Vector3d tmp(1.0, 0.0, 0.0);
  if (std::abs(tmp.dot(nn)) > 0.9) {
    tmp = Eigen::Vector3d(0.0, 1.0, 0.0);
  }
  Eigen::Vector3d t1 = nn.cross(tmp).normalized();
  Eigen::Vector3d t2 = nn.cross(t1);

  std::vector<Eigen::Vector3d> dirs;
  dirs.reserve(k);
  const double twoPi = 2.0 * M_PI;
  for (int i = 0; i < k; ++i) {
    double theta = twoPi * double(i) / double(k);
    Eigen::Vector3d t = std::cos(theta) * t1 + std::sin(theta) * t2;
    Eigen::Vector3d f = nn + mu * t;
    f.normalize();
    dirs.push_back(f);
  }
  return dirs;
}

Eigen::MatrixXd buildWrenchSet(const std::vector<Contact>& contacts,
                               int k, double wrenchScale = 1.0) {
  int M = int(contacts.size()) * k;
  Eigen::MatrixXd W(M, 6);
  int row = 0;
  for (const auto& c : contacts) {
    auto dirs = discretizeFrictionCone(c.n, c.mu, k);
    for (const auto& fdir : dirs) {
      Eigen::Vector3d f = wrenchScale * fdir;
      Eigen::Vector3d m = c.p.cross(f);
      W.row(row).segment<3>(0) = f;
      W.row(row).segment<3>(3) = m;
      ++row;
    }
  }
  return W;
}

Eigen::MatrixXd sampleUnitDirections(int dim, int numSamples) {
  std::mt19937 rng(42);
  std::normal_distribution<double> N(0.0, 1.0);
  Eigen::MatrixXd U(numSamples, dim);
  for (int i = 0; i < numSamples; ++i) {
    for (int j = 0; j < dim; ++j) {
      U(i, j) = N(rng);
    }
    U.row(i).normalize();
  }
  return U;
}

double epsilonQuality(const Eigen::MatrixXd& W, int numDirections) {
  if (W.rows() == 0) return 0.0;
  int M = int(W.rows());
  Eigen::MatrixXd U = sampleUnitDirections(6, numDirections); // (Nd,6)
  Eigen::MatrixXd proj = U * W.transpose(); // (Nd,M)
  double eps = std::numeric_limits<double>::infinity();
  for (int i = 0; i < proj.rows(); ++i) {
    double s = proj.row(i).maxCoeff();
    if (s < eps) eps = s;
  }
  return eps;
}

int main() {
  std::vector<Contact> contacts;
  Contact c1;
  c1.p = Eigen::Vector3d(0.05, 0.0, 0.0);
  c1.n = Eigen::Vector3d(-1.0, 0.0, 0.0).normalized();
  c1.mu = 0.7;
  contacts.push_back(c1);

  Contact c2;
  c2.p = Eigen::Vector3d(-0.05, 0.04, 0.0);
  c2.n = Eigen::Vector3d(1.0, -1.0, 0.0).normalized();
  c2.mu = 0.7;
  contacts.push_back(c2);

  Contact c3;
  c3.p = Eigen::Vector3d(-0.05, -0.04, 0.0);
  c3.n = Eigen::Vector3d(1.0, 1.0, 0.0).normalized();
  c3.mu = 0.7;
  contacts.push_back(c3);

  Eigen::MatrixXd W = buildWrenchSet(contacts, 8);
  double q = epsilonQuality(W, 512);
  std::cout << "Approx epsilon quality: " << q << std::endl;
  return 0;
}
      

This C++ implementation can be integrated into a real-time grasp planner running on a manipulator controller, with collision checking and kinematic constraints handled elsewhere.

Java is less common in robotics, but the same algorithm translates cleanly. Libraries such as EJML or Apache Commons Math can simplify matrix computations; here we use plain arrays to keep the core transparent.

import java.util.*;
import java.lang.Math;

class Contact {
    double[] p; // length 3
    double[] n; // length 3 (unit)
    double mu;
}

public class GraspScoring {

    static double[] normalize(double[] v) {
        double norm = 0.0;
        for (double x : v) norm += x * x;
        norm = Math.sqrt(norm);
        double[] out = new double[v.length];
        for (int i = 0; i < v.length; ++i) out[i] = v[i] / norm;
        return out;
    }

    static double[] cross(double[] a, double[] b) {
        return new double[] {
            a[1]*b[2] - a[2]*b[1],
            a[2]*b[0] - a[0]*b[2],
            a[0]*b[1] - a[1]*b[0]
        };
    }

    static double dot(double[] a, double[] b) {
        double s = 0.0;
        for (int i = 0; i < a.length; ++i) s += a[i] * b[i];
        return s;
    }

    static List<double[]> discretizeFrictionCone(double[] n, double mu, int k) {
        double[] nn = normalize(n);
        double[] tmp = new double[] {1.0, 0.0, 0.0};
        if (Math.abs(dot(tmp, nn)) > 0.9) {
            tmp = new double[] {0.0, 1.0, 0.0};
        }
        double[] t1 = normalize(cross(nn, tmp));
        double[] t2 = cross(nn, t1);

        List<double[]> dirs = new ArrayList<>(k);
        for (int i = 0; i < k; ++i) {
            double theta = 2.0 * Math.PI * ((double)i / (double)k);
            double[] t = new double[] {
                Math.cos(theta)*t1[0] + Math.sin(theta)*t2[0],
                Math.cos(theta)*t1[1] + Math.sin(theta)*t2[1],
                Math.cos(theta)*t1[2] + Math.sin(theta)*t2[2]
            };
            double[] f = new double[] {
                nn[0] + mu * t[0],
                nn[1] + mu * t[1],
                nn[2] + mu * t[2]
            };
            dirs.add(normalize(f));
        }
        return dirs;
    }

    static double[][] buildWrenchSet(List<Contact> contacts, int k, double wrenchScale) {
        int M = contacts.size() * k;
        double[][] W = new double[M][6];
        int row = 0;
        for (Contact c : contacts) {
            List<double[]> dirs = discretizeFrictionCone(c.n, c.mu, k);
            for (double[] fdir : dirs) {
                double[] f = new double[] {
                    wrenchScale * fdir[0],
                    wrenchScale * fdir[1],
                    wrenchScale * fdir[2]
                };
                double[] m = cross(c.p, f);
                for (int j = 0; j < 3; ++j) W[row][j] = f[j];
                for (int j = 0; j < 3; ++j) W[row][3 + j] = m[j];
                row++;
            }
        }
        return W;
    }

    static double[][] sampleUnitDirections(int dim, int numSamples, long seed) {
        Random rng = new Random(seed);
        double[][] U = new double[numSamples][dim];
        for (int i = 0; i < numSamples; ++i) {
            double norm = 0.0;
            for (int j = 0; j < dim; ++j) {
                double x = rng.nextGaussian();
                U[i][j] = x;
                norm += x * x;
            }
            norm = Math.sqrt(norm);
            for (int j = 0; j < dim; ++j) {
                U[i][j] /= norm;
            }
        }
        return U;
    }

    static double epsilonQuality(double[][] W, int numDirections) {
        if (W.length == 0) return 0.0;
        double[][] U = sampleUnitDirections(6, numDirections, 42L);
        double eps = Double.POSITIVE_INFINITY;
        for (int i = 0; i < U.length; ++i) {
            double maxProj = -Double.POSITIVE_INFINITY;
            for (int r = 0; r < W.length; ++r) {
                double s = 0.0;
                for (int j = 0; j < 6; ++j) {
                    s += U[i][j] * W[r][j];
                }
                if (s > maxProj) maxProj = s;
            }
            if (maxProj < eps) eps = maxProj;
        }
        return eps;
    }

    public static void main(String[] args) {
        List<Contact> contacts = new ArrayList<>();

        Contact c1 = new Contact();
        c1.p = new double[] {0.05, 0.0, 0.0};
        c1.n = normalize(new double[] {-1.0, 0.0, 0.0});
        c1.mu = 0.7;
        contacts.add(c1);

        Contact c2 = new Contact();
        c2.p = new double[] {-0.05, 0.04, 0.0};
        c2.n = normalize(new double[] {1.0, -1.0, 0.0});
        c2.mu = 0.7;
        contacts.add(c2);

        Contact c3 = new Contact();
        c3.p = new double[] {-0.05, -0.04, 0.0};
        c3.n = normalize(new double[] {1.0, 1.0, 0.0});
        c3.mu = 0.7;
        contacts.add(c3);

        double[][] W = buildWrenchSet(contacts, 8, 1.0);
        double q = epsilonQuality(W, 512);
        System.out.println("Approx epsilon quality: " + q);
    }
}
      

This Java implementation can be integrated with upstream perception and grasp-generation modules in a larger manipulation framework.

MATLAB is widely used in robotics for prototyping. The following script implements the same approximation. It can be wrapped as a MATLAB Function block inside a Simulink model, enabling grasp scoring within a closed-loop simulation.

function q = demo_grasp_score_matlab()
    % Example contacts
    contacts(1).p = [0.05; 0.0; 0.0];
    contacts(1).n = [-1.0; 0.0; 0.0];
    contacts(1).mu = 0.7;

    contacts(2).p = [-0.05; 0.04; 0.0];
    contacts(2).n = [1.0; -1.0; 0.0];
    contacts(2).mu = 0.7;

    contacts(3).p = [-0.05; -0.04; 0.0];
    contacts(3).n = [1.0; 1.0; 0.0];
    contacts(3).mu = 0.7;

    for i = 1:numel(contacts)
        contacts(i).n = contacts(i).n / norm(contacts(i).n);
    end

    W = build_wrench_set(contacts, 8, 1.0);
    q = epsilon_quality(W, 512);
    fprintf("Approx epsilon quality: %f\n", q);
end

function dirs = discretize_friction_cone(n, mu, k)
    nn = n / norm(n);
    tmp = [1.0; 0.0; 0.0];
    if abs(dot(tmp, nn)) > 0.9
        tmp = [0.0; 1.0; 0.0];
    end
    t1 = cross(nn, tmp);
    t1 = t1 / norm(t1);
    t2 = cross(nn, t1);

    dirs = zeros(3, k);
    for i = 1:k
        theta = 2.0 * pi * (i - 1) / k;
        t = cos(theta) * t1 + sin(theta) * t2;
        f = nn + mu * t;
        f = f / norm(f);
        dirs(:, i) = f;
    end
end

function W = build_wrench_set(contacts, k, wrench_scale)
    M = numel(contacts) * k;
    W = zeros(M, 6);
    row = 1;
    for idx = 1:numel(contacts)
        c = contacts(idx);
        dirs = discretize_friction_cone(c.n, c.mu, k);
        for j = 1:k
            f = wrench_scale * dirs(:, j);
            m = cross(c.p, f);
            W(row, 1:3) = f.';
            W(row, 4:6) = m.';
            row = row + 1;
        end
    end
end

function q = epsilon_quality(W, num_directions)
    if isempty(W)
        q = 0.0;
        return;
    end
    dim = 6;
    Nd = num_directions;
    % Sample Gaussian, then normalize rows
    X = randn(Nd, dim);
    norms = sqrt(sum(X.^2, 2));
    U = X ./ norms;
    proj = U * W.';  % Nd x M
    support_vals = max(proj, [], 2);
    q = min(support_vals);
end
      

In a Simulink environment, one can feed grasp descriptors (contact positions and normals) from a perception subsystem into the epsilon_quality function to dynamically score grasps in simulation.

Finally, we demonstrate an implementation in Wolfram Mathematica, which is convenient for symbolic experimentation and visualization of the grasp wrench space.

ClearAll[DiscretizeFrictionCone, ContactWrenches, BuildWrenchSet,
  SampleUnitDirections, EpsilonQuality];

DiscretizeFrictionCone[n_, mu_, k_Integer : 8] := Module[
  {nn, tmp, t1, t2, angles, dirs},
  nn = Normalize[n];
  tmp = {1., 0., 0.};
  If[Abs[tmp.nn] > 0.9, tmp = {0., 1., 0.}];
  t1 = Normalize[Cross[nn, tmp]];
  t2 = Cross[nn, t1];
  angles = N@Range[0, 2. Pi (1 - 1/k), 2. Pi/k];
  dirs = Table[
    Module[{t, f},
      t = Cos[theta] t1 + Sin[theta] t2;
      f = nn + mu t;
      Normalize[f]
    ],
    {theta, angles}
  ];
  dirs
];

ContactWrenches[p_, n_, mu_, k_Integer : 8, wrenchScale_ : 1.] := Module[
  {dirs, wrenches},
  dirs = DiscretizeFrictionCone[n, mu, k];
  wrenches = Table[
    Module[{f, m},
      f = wrenchScale dirs[[i]];
      m = Cross[p, f];
      Join[f, m]
    ],
    {i, Length[dirs]}
  ];
  wrenches
];

BuildWrenchSet[contacts_List, k_Integer : 8, wrenchScale_ : 1.] := Module[
  {wAll},
  wAll = Flatten[
    Table[
      ContactWrenches[
        contacts[[i, "p"]],
        Normalize[contacts[[i, "n"]]],
        contacts[[i, "mu"]],
        k, wrenchScale
      ],
      {i, Length[contacts]}
    ],
    1
  ];
  wAll
];

SampleUnitDirections[dim_Integer, numSamples_Integer] := Module[
  {x},
  x = RandomVariate[NormalDistribution[], {numSamples, dim}];
  x/Norm /@ x
];

EpsilonQuality[wrenches_List, numDirections_Integer : 256] := Module[
  {M, dim, dirs, proj, supportVals},
  M = Length[wrenches];
  If[M == 0, Return[0.]];
  dim = Length[First[wrenches]];
  dirs = SampleUnitDirections[dim, numDirections];
  proj = dirs.# & /@ wrenches; (* list of Nd-length vectors *)
  (* proj is effectively Transpose[dirs.wrenches^T] *)
  supportVals = Map[Max, Transpose[proj]];
  Min[supportVals]
];

(* Example usage *)
contacts = {
  <|"p" -> {0.05, 0., 0.}, "n" -> {-1., 0., 0.}, "mu" -> 0.7|>,
  <|"p" -> {-0.05, 0.04, 0.}, "n" -> {1., -1., 0.}, "mu" -> 0.7|>,
  <|"p" -> {-0.05, -0.04, 0.}, "n" -> {1., 1., 0.}, "mu" -> 0.7|>
};

Do[contacts[[i, "n"]] = Normalize[contacts[[i, "n"]]], {i, Length[contacts]}];

wSet = BuildWrenchSet[contacts, 8, 1.];
q = EpsilonQuality[wSet, 512];
Print["Approx epsilon quality: ", q];
      

Mathematica can additionally be used to visualize 2D or 3D projections of \( \mathcal{W}_g \) (for example, force-only or planar wrenches) and to experimentally verify geometric properties of the grasp quality metrics.

The following diagram shows how grasp scoring fits into a complete manipulation stack, emphasizing the separation between perception, candidate generation, scoring, and execution.

flowchart TD
  A["RGB-D / point cloud sensor"] --> B["Perception: object pose and shape"]
  B --> C["Grasp generator: propose candidate contact sets"]
  C --> D["Grasp score module: compute epsilon quality"]
  D --> E["Rank and filter grasps"]
  E --> F["Motion planner: plan collision-free arm trajectory"]
  F --> G["Controller / low-level torque control"]
        

This abstraction allows the same grasp scoring core to be reused across different robot platforms and software stacks.

Problem 1 (Scaling Property of \( Q_\epsilon \)): Let \( \mathcal{S}_g = \{\mathbf{w}_1,\dots,\mathbf{w}_M\} \) be the discrete wrench set for a grasp \( g \), and let \( \alpha > 0 \) be a scalar. Prove that if we scale all contact forces by \( \alpha \), then \( Q_\epsilon \) scales linearly: \( Q_\epsilon(\alpha g) = \alpha Q_\epsilon(g) \).

Solution:

Scaling all contact forces by \( \alpha \) scales each wrench by \( \alpha \), so the new discrete set is \( \mathcal{S}'_g = \{\alpha \mathbf{w}_1,\dots,\alpha \mathbf{w}_M\} \) and consequently \( \operatorname{conv}(\mathcal{S}'_g) = \alpha \operatorname{conv}(\mathcal{S}_g) \).

The Ferrari–Canny quality is the largest radius \( r \) such that \( r\mathcal{B} \subseteq \operatorname{conv}(\mathcal{S}_g) \). For the scaled grasp we require \( r'\mathcal{B} \subseteq \operatorname{conv}(\mathcal{S}'_g) = \alpha \operatorname{conv}(\mathcal{S}_g) \), which is equivalent to \( (r'/\alpha)\mathcal{B} \subseteq \operatorname{conv}(\mathcal{S}_g) \).

Thus the maximal feasible \( r' \) is \( r' = \alpha Q_\epsilon(g) \), proving \( Q_\epsilon(\alpha g) = \alpha Q_\epsilon(g) \).

Problem 2 (Support Function Approximation): Show that the sampling-based approximation \( \hat{Q}_\epsilon(g) = \min_j \max_{\mathbf{w} \in \mathcal{S}_g} \mathbf{u}_j^\top \mathbf{w} \) converges to the exact \( Q_\epsilon(g) \) as the number of directions \( N_d \) tends to infinity, assuming that directions are sampled uniformly on the unit sphere.

Solution:

For a convex set \( K \subset \mathbb{R}^n \), the distance from the origin to its boundary in direction \( \mathbf{u} \) is given by its support function

\[ h_K(\mathbf{u}) = \max_{\mathbf{x} \in K} \mathbf{u}^\top \mathbf{x}. \]

The distance from the origin to the closest supporting hyperplane is \( \min_{\|\mathbf{u}\|_2 = 1} h_K(\mathbf{u}) \). For \( K = \operatorname{conv}(\mathcal{S}_g) \), the exact \( Q_\epsilon(g) \) equals this minimum.

The approximation \( \hat{Q}_\epsilon(g) \) samples directions \( \mathbf{u}_j \) from the unit sphere. By uniform sampling and continuity of \( h_K \) on the compact sphere, the minimum over a dense sequence of sampled directions converges to the global minimum. Thus, as \( N_d \to \infty \), we have \( \hat{Q}_\epsilon(g) \to Q_\epsilon(g) \).

Problem 3 (2D Planar Grasp Example): Consider a planar object in \( \mathbb{R}^2 \) with two point contacts at positions \( p_1 = (1, 0)^\top \), \( p_2 = (-1, 0)^\top \), and normals \( n_1 = (-1, 0)^\top \), \( n_2 = (1, 0)^\top \) with frictionless contacts (no tangential forces). Show that this grasp cannot resist a vertical force \( (0, f_y)^\top \).

Solution:

With frictionless contacts, forces at the contacts must lie along the outward normals, i.e., \( f_1 = \lambda_1 n_1, \; f_2 = \lambda_2 n_2 \) with \( \lambda_1,\lambda_2 \ge 0 \). That is, \( f_1 = (-\lambda_1, 0)^\top \) and \( f_2 = (\lambda_2, 0)^\top \). Any linear combination yields net force \( f = f_1 + f_2 = (\lambda_2 - \lambda_1, 0)^\top \), which always has zero vertical component.

Hence no admissible combination of contact forces can generate \( (0, f_y)^\top \) with \( f_y \ne 0 \), so the grasp cannot resist vertical disturbances, and any reasonable metric \( Q(g) \) must reflect very low quality for such directions.

Problem 4 (Planar Moment Balance): Extend Problem 3 by considering planar wrenches \( (f_x, f_y, m_z)^\top \). Show that with the same two frictionless symmetric contacts, you can generate pure moments \( (0, 0, m_z)^\top \) but still cannot resist arbitrary planar wrenches.

Solution:

The contact forces are the same as in Problem 3. Moments (about the origin) are \( m_1 = p_1 \times f_1 \), \( m_2 = p_2 \times f_2 \). In planar coordinates, \( p_i = (x_i, y_i)^\top \), \( f_i = (f_{ix}, f_{iy})^\top \), and \( m_i = x_i f_{iy} - y_i f_{ix} \). Here \( y_1 = y_2 = 0 \), so

\[ m_1 = x_1 f_{1y} - 0\cdot f_{1x} = 0,\quad m_2 = x_2 f_{2y} - 0\cdot f_{2x} = 0, \]

because all forces have zero vertical component. Thus net moment is also zero: \( m_z = 0 \). Therefore even pure moments cannot be generated; the set of achievable planar wrenches is only the horizontal axis \( (f_x, 0, 0)^\top \). This grasp is extremely poor in terms of wrench space coverage, and \( Q_\epsilon(g) = 0 \).

Problem 5 (Algorithm Flow Check): Explain why, in the pipeline of Section 3, we must compute the support function for all sampled directions \( \mathbf{u}_j \), instead of only directions aligned with the wrenches in \( \mathcal{S}_g \).

Solution:

The closest supporting hyperplane of a convex polytope does not in general correspond to a direction aligned with any vertex or generating vector. The minimal value of the support function may occur for a direction that is a combination of face normals. Limiting to directions aligned with elements of \( \mathcal{S}_g \) might miss the true worst-case direction and thus overestimate \( Q_\epsilon(g) \). Sampling a dense set of directions on the unit sphere approximates the continuum of possible hyperplane orientations and yields a more accurate estimate.

In this lab, we operationalized the theory of grasp wrench space and Ferrari–Canny \( \epsilon \)-quality into concrete algorithms and multi-language implementations. We:

• Defined the discrete wrench set via friction cone discretization and contact moment generation.
• Used the support function of the convex hull to approximate the largest origin-centered inscribed ball, yielding a robust grasp score.
• Implemented the scoring pipeline in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, enabling integration into diverse robotics toolchains.

These computational tools are the backbone of analytic grasp selection modules and form the basis for hybrid analytic/data-driven grasping approaches discussed in subsequent chapters.