Recall that a wrench on a rigid body collects a 3D force and a 3D torque about some reference point into a 6D vector \( \mathbf{w} \in \mathbb{R}^6 \). If \( \mathbf{f} \in \mathbb{R}^3 \) is the resultant force and \( \mathbf{\tau} \in \mathbb{R}^3 \) is the resultant torque about the object frame origin, we write

\[ \mathbf{w} = \begin{bmatrix} \mathbf{f} \\ \boldsymbol{\tau} \end{bmatrix} \in \mathbb{R}^6. \]

For a multi-finger grasp with frictional contacts, each contact can generate forces (and possibly moments) constrained by its friction cone and actuation limits. These contact forces are mapped, via rigid-body kinematics, to a net wrench \( \mathbf{w} \) on the object. The grasp wrench space is the set of all wrenches that can be produced by admissible contact forces.

Intuitively:

• Each contact defines a local set of achievable forces (a friction cone).
• These forces are mapped to wrench vectors at the object frame.
• The combination of all such wrenches, under nonnegative scaling and limits, forms a convex cone or polytope in \(\mathbb{R}^6\).
• Force-closure corresponds to the origin being in the interior of this set.

flowchart TD
  A["Contact geometry (positions, normals)"]
    --> B["Local friction cones at contacts"]
  B --> C["Linearize cones into finite force directions"]
  C --> D["Map forces to object-frame wrenches (G matrix)"]
  D --> E["Generate primitive wrenches in R6"]
  E --> F["Convex hull / cone => Grasp Wrench Space"]
  F --> G["Compute quality metrics (volume, epsilon, condition)"]
        

Consider a grasp with \( m \) contacts. For contact \( i \), let:

• \( \mathcal{C}_i \) be the local contact frame.
• \( \mathbf{R}_i \in SO(3) \) be the rotation from \( \mathcal{C}_i \) to the object frame.
• \( \mathbf{p}_i \in \mathbb{R}^3 \) be the contact point position in the object frame.
• \( \mathbf{f}_i^{\mathcal{C}_i} \in \mathbb{R}^3 \) be the contact force expressed in \( \mathcal{C}_i \).

The force in the object frame is \( \mathbf{f}_i = \mathbf{R}_i \mathbf{f}_i^{\mathcal{C}_i} \). The torque about the object origin is \( \boldsymbol{\tau}_i = \mathbf{p}_i \times \mathbf{f}_i \). With the skew-symmetric matrix

\[ [\mathbf{p}_i]_{\times} = \begin{bmatrix} 0 & -p_{i,z} & p_{i,y} \\ p_{i,z} & 0 & -p_{i,x} \\ -p_{i,y} & p_{i,x} & 0 \end{bmatrix}, \]

we can write the contact wrench induced by \( \mathbf{f}_i^{\mathcal{C}_i} \) in linear form:

\[ \mathbf{w}_i = \begin{bmatrix} \mathbf{f}_i \\ \boldsymbol{\tau}_i \end{bmatrix} = \begin{bmatrix} \mathbf{R}_i \\ [\mathbf{p}_i]_{\times} \mathbf{R}_i \end{bmatrix} \mathbf{f}_i^{\mathcal{C}_i} \triangleq G_i\,\mathbf{f}_i^{\mathcal{C}_i}, \quad G_i \in \mathbb{R}^{6\times 3}. \]

Stacking all contact forces into a vector \( \mathbf{f} = [(\mathbf{f}_1^{\mathcal{C}_1})^\top,\dots,(\mathbf{f}_m^{\mathcal{C}_m})^\top]^\top \in \mathbb{R}^{3m} \), and all maps as \( G = [G_1,\dots,G_m] \in \mathbb{R}^{6\times 3m} \), we obtain the grasp map:

\[ \mathbf{w} = \sum_{i=1}^m \mathbf{w}_i = \sum_{i=1}^m G_i \mathbf{f}_i^{\mathcal{C}_i} = G \mathbf{f}. \]

The matrix \( G \) maps admissible contact forces to their resultant wrench on the object. A necessary condition for force-closure is \( \operatorname{rank}(G) = 6 \), but this is not sufficient because friction cone constraints restrict \( \mathbf{f} \).

At each contact, Coulomb friction constrains the admissible contact force. In the local frame \( \mathcal{C}_i \), decompose \( \mathbf{f}_i^{\mathcal{C}_i} \) into normal and tangential components:

\[ \mathbf{f}_i^{\mathcal{C}_i} = f_{n,i}\,\mathbf{n}_i + \mathbf{f}_{t,i}, \quad \mathbf{n}_i \in \mathbb{R}^3,\ \|\mathbf{n}_i\|_2 = 1,\ \mathbf{f}_{t,i} \perp \mathbf{n}_i. \]

With friction coefficient \( \mu_i > 0 \), the (ideal) friction cone is

\[ \mathcal{F}_i = \left\{ \mathbf{f}_i^{\mathcal{C}_i} \,\middle|\, f_{n,i} \ge 0,\ \|\mathbf{f}_{t,i}\|_2 \le \mu_i f_{n,i} \right\}, \]

a second-order (convex) cone in \(\mathbb{R}^3\). For algorithmic planning, it is standard to approximate \(\mathcal{F}_i\) by a polyhedral cone generated by a finite set of unit directions \( \{\mathbf{d}_{i,1},\dots,\mathbf{d}_{i,k_i}\} \subset \mathbb{R}^3 \) contained inside the true friction cone:

\[ \tilde{\mathcal{F}}_i = \left\{ \sum_{j=1}^{k_i} \alpha_{i,j} \mathbf{d}_{i,j} \,\middle|\, \alpha_{i,j} \ge 0 \right\} = \operatorname{cone}\{\mathbf{d}_{i,1},\dots,\mathbf{d}_{i,k_i}\}. \]

For each generating direction \(\mathbf{d}_{i,j}\), we obtain a primitive contact wrench

\[ \mathbf{w}_{i,j} = G_i\,\mathbf{d}_{i,j} \in \mathbb{R}^6. \]

Collecting all primitive wrenches in the set \( \mathcal{W}_0 = \{\mathbf{w}_{i,j}\}_{i=1,\dots,m}^{j=1,\dots,k_i} \), any wrench achievable with the polyhedral approximation can be written as

\[ \mathbf{w} = \sum_{i=1}^m \sum_{j=1}^{k_i} \alpha_{i,j}\,\mathbf{w}_{i,j}, \quad \alpha_{i,j} \ge 0. \]

Hence the conic grasp wrench space is

\[ \mathcal{W}_c = \left\{ \sum_{i=1}^m \sum_{j=1}^{k_i} \alpha_{i,j}\,\mathbf{w}_{i,j} \,\middle|\, \alpha_{i,j} \ge 0 \right\} = \operatorname{cone}(\mathcal{W}_0), \]

which is a convex cone in \(\mathbb{R}^6\).

Often, grasp quality is evaluated over a normalized wrench set by restricting the total “effort” to unity:

\[ \mathcal{W}_1 = \left\{ \sum_{k=1}^N \lambda_k \mathbf{w}_k \,\middle|\, \lambda_k \ge 0,\ \sum_{k=1}^N \lambda_k = 1 \right\} = \operatorname{conv}(\mathcal{W}_0), \quad N = \sum_{i=1}^m k_i. \]

Thus \(\mathcal{W}_1\) is the convex hull of the primitive wrenches: a compact convex polytope in \(\mathbb{R}^6\), commonly called the grasp wrench space (GWS) in planning literature.

A grasp has force-closure if the fingers can generate a wrench that balances any external wrench on the object (within some scaling), while respecting friction constraints and unilateral contact.

When friction cones are modeled as polyhedral and the contacts are unilateral, a fundamental result states:

Theorem (Nguyen-type criterion). Let \(\mathcal{W}_1 = \operatorname{conv}(\mathcal{W}_0)\) be the normalized grasp wrench space defined above. Then, under mild nondegeneracy assumptions, the grasp is force-closure if and only if the origin \( \mathbf{0} \) lies in the interior of \( \mathcal{W}_1 \).

Sketch of proof idea.

• If \( \mathbf{0} \in \operatorname{int}(\mathcal{W}_1) \), then for every unit direction \( \mathbf{v} \in \mathbb{R}^6 \) there exist wrenches in \(\mathcal{W}_1\) with positive and negative projection on \(\mathbf{v}\). By scaling (using the cone \(\mathcal{W}_c\)), we can produce arbitrarily large positive or negative components along any direction in wrench space; thus any external wrench can be balanced.
• Conversely, if the origin is not in the interior of \(\mathcal{W}_1\), then by the separating hyperplane theorem there exists some nonzero \( \mathbf{v} \) such that \( \mathbf{v}^\top \mathbf{w} \le 0 \) for all \( \mathbf{w} \in \mathcal{W}_1 \). No admissible combination of contact forces can create a wrench with positive projection along \( \mathbf{v} \), so certain external wrenches cannot be balanced.

This geometric view motivates quality metrics based on the “size” and “shape” of \(\mathcal{W}_1\) around the origin.

Let \(\mathcal{W}_1 = \operatorname{conv}(\mathcal{W}_0)\) be the GWS. A simple idea is: larger GWS volume around the origin suggests better ability to resist disturbances. For a polytope in \(\mathbb{R}^6\), the (6-dimensional) volume can in principle be computed by decomposing the polytope into simplices with vertices among the primitive wrenches and the origin.

For a simplex \( S = \operatorname{conv}\{\mathbf{0},\mathbf{w}_{i_1},\dots,\mathbf{w}_{i_6}\} \) in \(\mathbb{R}^6\), the volume is

\[ \operatorname{vol}_6(S) = \frac{1}{6!}\left|\det\left( [\mathbf{w}_{i_1},\dots,\mathbf{w}_{i_6}] \right)\right|. \]

Summing over a triangulation of \(\mathcal{W}_1\) yields a volume-based metric \( Q_{\text{vol}} \). In practice, computing an exact 6D volume is expensive and numerically delicate, so approximations (e.g., Monte Carlo) are common.

A related family of metrics uses the grasp map \( G \in \mathbb{R}^{6\times 3m} \) directly. For example, define the isotropy index

\[ Q_{\text{iso}} = \frac{\sigma_{\min}(G)}{\sigma_{\max}(G)}, \]

where \(\sigma_{\min}\) and \(\sigma_{\max}\) are the smallest and largest singular values of \(\,G\). If \(G\) is well conditioned (i.e., \(\sigma_{\min}\) not too small compared to \(\sigma_{\max}\)), then the grasp is roughly “directionally uniform” in its ability to generate wrenches.

Another determinant-based metric is

\[ Q_{\det} = \sqrt{\det(G G^\top)}, \]

which (loosely) measures the volume of the image of a unit ball in contact force space through the linear map \(G\). All of these metrics, however, depend on force scaling and units, and they do not explicitly encode the friction-cone constraints; the \(\epsilon\)-metric does.

A widely used quality measure is the \(\epsilon\)-metric proposed by Ferrari and Canny. Let \(\mathcal{W}_1\) be the normalized GWS. Define

\[ Q_{\epsilon} = \max_{\epsilon \ge 0} \left\{ \epsilon \,\middle|\, B_2(\mathbf{0},\epsilon) \subseteq \mathcal{W}_1 \right\}, \]

where \(B_2(\mathbf{0},\epsilon)\) is the closed Euclidean ball of radius \(\epsilon\) centered at the origin in \(\mathbb{R}^6\). Geometrically, \( Q_{\epsilon} \) is the radius of the largest ball centered at the origin that fits inside the GWS.

Using the dual characterization of convex sets via support functions, one can show that

\[ Q_{\epsilon} = \min_{\|\mathbf{v}\|_2 = 1} \max_{\mathbf{w} \in \mathcal{W}_1} \mathbf{v}^\top \mathbf{w}. \]

Interpretation. For each unit direction \( \mathbf{v} \) in wrench space, the inner maximization \( \max_{\mathbf{w} \in \mathcal{W}_1} \mathbf{v}^\top \mathbf{w} \) is the largest wrench component achievable along \(\mathbf{v}\). The outer minimization picks the worst-case direction. Thus \( Q_{\epsilon} \) is the minimum over directions of the maximum resistible disturbance component along that direction, under the normalization used to define \(\mathcal{W}_1\).

In particular:

• \( Q_{\epsilon} > 0 \) if and only if the grasp is force-closure.
• \( Q_{\epsilon} \) gives a margin of robustness: larger values mean that the grasp can resist larger unknown disturbances in any direction.

For a polytope described as the convex hull of primitive wrenches, the support function simplifies to

\[ \max_{\mathbf{w} \in \mathcal{W}_1} \mathbf{v}^\top \mathbf{w} = \max_{k=1,\dots,N} \mathbf{v}^\top \mathbf{w}_k, \]

so the \(\epsilon\)-metric can be approximated by sampling many unit directions \(\mathbf{v}\) on the unit sphere \(S^5\) and evaluating this maximum.

A typical computational pipeline for grasp quality evaluation using linearized friction cones is:

1. Compute contact frames \(\mathcal{C}_i\), contact points \(\mathbf{p}_i\), and rotations \(\mathbf{R}_i\).
2. For each contact, choose \(k_i\) directions \(\mathbf{d}_{i,j}\) inside the friction cone (e.g., equally spaced in tangential angle).
3. Compute primitive wrenches \(\mathbf{w}_{i,j} = G_i \mathbf{d}_{i,j}\).
4. Construct the GWS polytope \(\mathcal{W}_1 = \operatorname{conv}(\mathcal{W}_0)\) using a convex hull algorithm (e.g., Qhull).
5. Check whether \(\mathbf{0}\) is inside the polytope (force-closure test).
6. Compute quality metrics: volume approximations, matrix-based indices, and \(\epsilon\)-metric (e.g., by evaluating distances from the origin to polytope facets or by directional sampling).

flowchart TD
  S["Input: contact points, normals, mu"]
    --> C1["Linearize each friction cone"]
  C1 --> C2["Compute primitive wrenches in R6"]
  C2 --> H["Convex hull to build GWS polytope"]
  H --> FC["Test origin inside polytope (force closure)"]
  H --> QM["Compute metrics: volume, epsilon, isotropy"]
  QM --> O["Output: grasp quality scores"]
        

Exact computation of \(\epsilon\) in high dimensions involves solving linear or convex programs derived from the polytope facet description; for planning, approximate methods (sampling directions, low-dimensional projections) are often sufficient.

The following Python code illustrates the computation of primitive wrenches, a simple isotropy metric using the grasp map, and a sampled approximation of the \(\epsilon\)-metric. In practice you would integrate with robotic simulation libraries (e.g., PyBullet, MuJoCo bindings, or custom kinematics) to obtain contact locations and normals.

import numpy as np
from numpy.linalg import svd, norm

# -------------------------------------------------------
# Contact and wrench utilities
# -------------------------------------------------------

def skew(p):
    """Return 3x3 skew-symmetric matrix for p in R^3."""
    px, py, pz = p
    return np.array([[0.0, -pz,  py],
                     [pz,  0.0, -px],
                     [-py, px,  0.0]])

def grasp_map_block(p_i, R_i):
    """
    Build 6x3 grasp map block G_i for a point contact at p_i (object frame),
    with local-to-object rotation R_i.
    """
    upper = R_i
    lower = skew(p_i) @ R_i
    return np.vstack((upper, lower))

def linearized_friction_directions(mu, k_dirs=8):
    """
    Build k_dirs generator directions for a friction cone with coefficient mu.
    Returns 3 x k_dirs matrix whose columns are unit directions in local frame.
    Normal is along +z in local coordinates.
    """
    dirs = []
    # Tangential radius in local coordinates
    alpha = np.arctan(mu)
    # Normal component cos(alpha), tangential radius sin(alpha)
    fn = np.cos(alpha)
    ft = np.sin(alpha)
    for j in range(k_dirs):
        theta = 2.0 * np.pi * j / k_dirs
        tx = ft * np.cos(theta)
        ty = ft * np.sin(theta)
        # Normal along z-axis
        dirs.append(np.array([tx, ty, fn]))
    return np.column_stack(dirs)

def primitive_wrenches(p_list, R_list, mu_list, k_dirs=8):
    """
    Compute primitive wrenches for all contacts.
    p_list: list of 3-vectors (object frame contact points)
    R_list: list of 3x3 rotation matrices (local->object)
    mu_list: list of friction coefficients
    Returns:
      W: 6 x N matrix of primitive wrenches
    """
    W_cols = []
    for p_i, R_i, mu_i in zip(p_list, R_list, mu_list):
        G_i = grasp_map_block(p_i, R_i)
        D_i = linearized_friction_directions(mu_i, k_dirs=k_dirs)
        # Each column of D_i gives a primitive wrench
        W_i = G_i @ D_i
        for j in range(W_i.shape[1]):
            W_cols.append(W_i[:, j])
    return np.column_stack(W_cols)  # 6 x N

# -------------------------------------------------------
# Matrix-based metric from G
# -------------------------------------------------------

def grasp_map_from_primitive(W):
    """
    Construct a simple contact-force->wrench map from primitive wrenches W.
    Here we treat each primitive direction as an independent actuator
    (purely for quality metrics like isotropy).
    W: 6 x N matrix of primitive wrenches
    """
    # If primitive forces are unit-norm, G can be approximated by W itself
    return W

def isotropy_metric(G):
    """
    Q_iso = sigma_min(G) / sigma_max(G).
    """
    U, s, Vt = svd(G, full_matrices=False)
    s_min, s_max = np.min(s), np.max(s)
    if s_max < 1e-12:
        return 0.0
    return float(s_min / s_max)

# -------------------------------------------------------
# Sampled epsilon metric approximation
# -------------------------------------------------------

def epsilon_metric_sampled(W, n_directions=2000, rng=None):
    """
    Approximate epsilon metric by sampling directions on the unit sphere in R^6.
    W: 6 x N matrix of primitive wrenches (defining conv(W))
    Returns:
      q_eps: approximate epsilon value
    """
    if rng is None:
        rng = np.random.default_rng()

    # Normalize primitive wrenches (for a unit "effort" polytope)
    Wn = W / np.maximum(norm(W, axis=0, keepdims=True), 1e-12)

    q_eps = np.inf
    for _ in range(n_directions):
        v = rng.normal(size=6)
        v /= norm(v)
        # Support value in direction v: max_k v^T w_k
        support = np.max(v @ Wn)
        q_eps = min(q_eps, support)
    return float(q_eps)

# -------------------------------------------------------
# Example usage
# -------------------------------------------------------

if __name__ == "__main__":
    # Example: 3-finger grasp on a spherical object
    # Contact points (object frame)
    p_list = [
        np.array([0.05, 0.0, 0.0]),
        np.array([-0.025, 0.043301, 0.0]),
        np.array([-0.025, -0.043301, 0.0]),
    ]
    # Local normals pointing approximately toward the origin
    R_list = []
    for p in p_list:
        n = -p / (norm(p) + 1e-12)
        # Build a simple frame with n as z-axis
        z = n
        # Pick arbitrary x-axis not parallel to z
        tmp = np.array([1.0, 0.0, 0.0])
        if abs(np.dot(tmp, z)) > 0.9:
            tmp = np.array([0.0, 1.0, 0.0])
        x = tmp - np.dot(tmp, z) * z
        x /= norm(x)
        y = np.cross(z, x)
        R = np.column_stack((x, y, z))
        R_list.append(R)

    mu_list = [0.8, 0.8, 0.8]

    W = primitive_wrenches(p_list, R_list, mu_list, k_dirs=8)
    G = grasp_map_from_primitive(W)

    q_iso = isotropy_metric(G)
    q_eps = epsilon_metric_sampled(W, n_directions=2000)

    print("Isotropy metric Q_iso:", q_iso)
    print("Approximate epsilon metric Q_eps:", q_eps)
      

The sampled \(\epsilon\)-metric converges (in probability) to the true value as the number of random directions increases. The isotropy metric uses the primitive wrenches as a surrogate for the full grasp map.

In C++, we can use the Eigen library for linear algebra. Below is a minimal implementation of grasp map blocks and a simple isotropy metric based on the singular values of a grasp map approximated from primitive wrenches.

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

using Vec3 = Eigen::Vector3d;
using Mat3 = Eigen::Matrix3d;
using Mat63 = Eigen::Matrix<double, 6, 3>;
using Mat6X = Eigen::Matrix<double, 6, Eigen::Dynamic>;

Mat3 skew(const Vec3& p) {
    Mat3 S;
    S << 0.0,   -p.z(),  p.y(),
          p.z(),  0.0,   -p.x(),
         -p.y(),  p.x(),  0.0;
    return S;
}

Mat63 graspMapBlock(const Vec3& p_i, const Mat3& R_i) {
    Mat63 G_i;
    G_i.block<3,3>(0,0) = R_i;
    G_i.block<3,3>(3,0) = skew(p_i) * R_i;
    return G_i;
}

Eigen::MatrixXd linearizedFrictionDirections(double mu, int k_dirs = 8) {
    Eigen::MatrixXd D(3, k_dirs);
    double alpha = std::atan(mu);
    double fn = std::cos(alpha);
    double ft = std::sin(alpha);
    for (int j = 0; j < k_dirs; ++j) {
        double theta = 2.0 * M_PI * j / static_cast<double>(k_dirs);
        double tx = ft * std::cos(theta);
        double ty = ft * std::sin(theta);
        D.col(j) = Vec3(tx, ty, fn);
    }
    return D;
}

Mat6X primitiveWrenches(const std::vector<Vec3>& p_list,
                        const std::vector<Mat3>& R_list,
                        const std::vector<double>& mu_list,
                        int k_dirs = 8)
{
    int m = static_cast<int>(p_list.size());
    int N = m * k_dirs;
    Mat6X W(6, N);
    int col = 0;
    for (int i = 0; i < m; ++i) {
        Mat63 G_i = graspMapBlock(p_list[i], R_list[i]);
        Eigen::MatrixXd D_i = linearizedFrictionDirections(mu_list[i], k_dirs);
        Mat6X W_i = G_i * D_i;
        for (int j = 0; j < k_dirs; ++j) {
            W.col(col++) = W_i.col(j);
        }
    }
    return W;
}

double isotropyMetric(const Mat6X& G) {
    Eigen::JacobiSVD<Mat6X> svd(G, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::VectorXd s = svd.singularValues();
    double s_min = s.minCoeff();
    double s_max = s.maxCoeff();
    if (s_max < 1e-12) return 0.0;
    return s_min / s_max;
}

int main() {
    // Example: trivial 3-contact setup (placeholder geometry)
    std::vector<Vec3> p_list;
    std::vector<Mat3> R_list;
    std::vector<double> mu_list;

    p_list.emplace_back(0.05, 0.0, 0.0);
    p_list.emplace_back(-0.025, 0.043301, 0.0);
    p_list.emplace_back(-0.025, -0.043301, 0.0);

    for (const auto& p : p_list) {
        Vec3 n = -p.normalized();
        Vec3 tmp(1.0, 0.0, 0.0);
        if (std::abs(tmp.dot(n)) > 0.9) {
            tmp = Vec3(0.0, 1.0, 0.0);
        }
        Vec3 x = (tmp - tmp.dot(n) * n).normalized();
        Vec3 y = n.cross(x);
        Mat3 R;
        R.col(0) = x;
        R.col(1) = y;
        R.col(2) = n;
        R_list.push_back(R);
        mu_list.push_back(0.8);
    }

    Mat6X W = primitiveWrenches(p_list, R_list, mu_list, 8);
    double q_iso = isotropyMetric(W);

    std::cout << "Isotropy metric Q_iso = " << q_iso << std::endl;
    return 0;
}
      

Extensions would include interfacing with a convex hull library (e.g., Qhull, CGAL) to build the full GWS polytope and computing \(\epsilon\) by examining distances from the origin to its facets.

In Java, one can use a numerical library such as EJML for matrix operations. Below is a conceptual example using EJML-style interfaces to compute primitive wrenches and a simple isotropy metric.

import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.SingularOps_DDRM;
import org.ejml.dense.row.decomposition.svd.SingularValueDecomposition_DDRM;
import org.ejml.dense.row.decomposition.svd.SvdImplicitQrDecompose_DDRM;

public class GraspQuality {

    public static DMatrixRMaj skew(double[] p) {
        DMatrixRMaj S = new DMatrixRMaj(3,3);
        double px = p[0], py = p[1], pz = p[2];
        S.set(0,0, 0.0);   S.set(0,1, -pz);  S.set(0,2,  py);
        S.set(1,0,  pz);   S.set(1,1, 0.0);  S.set(1,2, -px);
        S.set(2,0, -py);   S.set(2,1,  px);  S.set(2,2, 0.0);
        return S;
    }

    public static DMatrixRMaj graspMapBlock(double[] p_i, DMatrixRMaj R_i) {
        DMatrixRMaj G_i = new DMatrixRMaj(6,3);
        // Upper block = R_i
        for (int r = 0; r < 3; ++r) {
            for (int c = 0; c < 3; ++c) {
                G_i.set(r, c, R_i.get(r, c));
            }
        }
        // Lower block = skew(p_i) * R_i
        DMatrixRMaj S = skew(p_i);
        DMatrixRMaj SR = new DMatrixRMaj(3,3);
        CommonOps_DDRM.mult(S, R_i, SR);
        for (int r = 0; r < 3; ++r) {
            for (int c = 0; c < 3; ++c) {
                G_i.set(r + 3, c, SR.get(r, c));
            }
        }
        return G_i;
    }

    public static DMatrixRMaj linearizedFrictionDirections(double mu, int kDirs) {
        DMatrixRMaj D = new DMatrixRMaj(3, kDirs);
        double alpha = Math.atan(mu);
        double fn = Math.cos(alpha);
        double ft = Math.sin(alpha);
        for (int j = 0; j < kDirs; ++j) {
            double theta = 2.0 * Math.PI * j / (double) kDirs;
            double tx = ft * Math.cos(theta);
            double ty = ft * Math.sin(theta);
            D.set(0, j, tx);
            D.set(1, j, ty);
            D.set(2, j, fn);
        }
        return D;
    }

    public static double isotropyMetric(DMatrixRMaj G) {
        SingularValueDecomposition_DDRM svd =
            new SvdImplicitQrDecompose_DDRM(true, true, true, false);
        if (!svd.decompose(G)) {
            return 0.0;
        }
        double[] sv = svd.getSingularValues();
        double sMin = Double.POSITIVE_INFINITY;
        double sMax = 0.0;
        for (double s : sv) {
            sMin = Math.min(sMin, s);
            sMax = Math.max(sMax, s);
        }
        if (sMax < 1e-12) return 0.0;
        return sMin / sMax;
    }

    public static void main(String[] args) {
        // For brevity, contact geometry setup omitted; assume we have a
        // 6 x N primitive wrench matrix G_approx.

        // Example placeholder: random 6 x 12 matrix
        DMatrixRMaj G = new DMatrixRMaj(6, 12);
        java.util.Random rng = new java.util.Random(0);
        for (int i = 0; i < G.getNumElements(); ++i) {
            G.set(i, rng.nextGaussian());
        }

        double qIso = isotropyMetric(G);
        System.out.println("Isotropy metric Q_iso = " + qIso);
    }
}
      

For full GWS computation in Java you would integrate collision/contact computation (e.g., from a physics engine) and a convex hull library to build the polytope and compute advanced metrics like \(\epsilon\).

MATLAB provides convenient linear algebra operations and can be embedded in Simulink using MATLAB Function blocks. Below is a script to compute primitive wrenches and a sampled \(\epsilon\)-metric.

function grasp_quality_demo()
    % Example contact geometry
    p_list = [ 0.05,    0.0,      0.0;
              -0.025,   0.043301, 0.0;
              -0.025,  -0.043301, 0.0 ];
    mu_list = [0.8, 0.8, 0.8];
    m = size(p_list,1);
    R_list = cell(m,1);

    for i = 1:m
        p = p_list(i,:).';
        n = -p / norm(p);
        tmp = [1; 0; 0];
        if abs(tmp.' * n) > 0.9
            tmp = [0; 1; 0];
        end
        x = tmp - (tmp.'*n)*n;
        x = x / norm(x);
        y = cross(n, x);
        R = [x, y, n];
        R_list{i} = R;
    end

    k_dirs = 8;
    W = primitive_wrenches(p_list, R_list, mu_list, k_dirs);

    % Isotropy metric via SVD of W
    s = svd(W);
    q_iso = min(s) / max(s);

    % Sampled epsilon metric
    q_eps = epsilon_sampled(W, 2000);

    fprintf('Isotropy metric Q_iso = %.4f\n', q_iso);
    fprintf('Approx epsilon metric Q_eps = %.4f\n', q_eps);
end

function S = skew(p)
    px = p(1); py = p(2); pz = p(3);
    S = [  0,  -pz,  py;
          pz,   0,  -px;
         -py,  px,   0 ];
end

function G_i = grasp_map_block(p_i, R_i)
    upper = R_i;
    lower = skew(p_i) * R_i;
    G_i = [upper; lower];
end

function D = linearized_friction_directions(mu, k_dirs)
    alpha = atan(mu);
    fn = cos(alpha);
    ft = sin(alpha);
    D = zeros(3, k_dirs);
    for j = 1:k_dirs
        theta = 2*pi*(j-1)/k_dirs;
        tx = ft * cos(theta);
        ty = ft * sin(theta);
        D(:,j) = [tx; ty; fn];
    end
end

function W = primitive_wrenches(p_list, R_list, mu_list, k_dirs)
    m = size(p_list,1);
    N = m * k_dirs;
    W = zeros(6, N);
    col = 1;
    for i = 1:m
        p_i = p_list(i,:).';
        R_i = R_list{i};
        G_i = grasp_map_block(p_i, R_i);
        D_i = linearized_friction_directions(mu_list(i), k_dirs);
        W_i = G_i * D_i;
        W(:, col:col+k_dirs-1) = W_i;
        col = col + k_dirs;
    end
end

function q_eps = epsilon_sampled(W, n_directions)
    % Normalize primitive wrenches to form a "unit" polytope
    norms = sqrt(sum(W.^2, 1));
    Wn = W ./ max(norms, 1e-12);

    q_eps = inf;
    for k = 1:n_directions
        v = randn(6,1);
        v = v / norm(v);
        support = max(v.' * Wn);
        q_eps = min(q_eps, support);
    end
end
      

In Simulink, wrap the core computations (primitive_wrenches, epsilon_sampled) inside MATLAB Function blocks, fed by contact point and normal signals from the robot dynamics model. This enables online evaluation of grasp quality within closed-loop simulations.

Wolfram Mathematica has built-in support for convex hulls and region-based computations, which is convenient for GWS analysis in lower-dimensional embeddings or projected wrench spaces.

(* Skew-symmetric matrix *)
skew[p_List] := {
  {0, -p[[3]], p[[2]]},
  {p[[3]], 0, -p[[1]]},
  {-p[[2]], p[[1]], 0}
};

(* Grasp map block *)
graspMapBlock[p_List, R_?MatrixQ] := Module[{upper, lower},
  upper = R;
  lower = skew[p].R;
  ArrayFlatten[{ {upper}, {lower} }]
];

(* Linearized friction directions (local frame) *)
linearizedFrictionDirections[mu_, kDirs_:8] := Module[
  {alpha, fn, ft, dirs},
  alpha = ArcTan[mu];
  fn = Cos[alpha]; ft = Sin[alpha];
  dirs = Table[
    With[{theta = 2. Pi j/kDirs},
      {ft Cos[theta], ft Sin[theta], fn}
    ],
    {j, 0, kDirs - 1}
  ];
  Transpose[dirs]
];

(* Primitive wrenches *)
primitiveWrenches[pList_List, RList_List, muList_List, kDirs_:8] :=
 Module[{m, cols},
  m = Length[pList];
  cols = Flatten[
    Table[
      Module[{G, D, Wi},
        G = graspMapBlock[pList[[i]], RList[[i]]];
        D = linearizedFrictionDirections[muList[[i]], kDirs];
        Wi = G.D;
        Table[Wi[[All, j]], {j, 1, kDirs}]
      ],
      {i, 1, m}
    ],
    1
  ];
  Transpose[cols] (* 6 x N *)
];

(* Sampled epsilon metric *)
epsilonSampled[W_?MatrixQ, nDirections_:2000] := Module[
  {norms, Wn, qeps, v, support},
  norms = Sqrt[Total[W^2, {1}]];
  Wn = W/Max[norms, 10.^(-12)];
  qeps = Infinity;
  Do[
    v = RandomVariate[NormalDistribution[], 6];
    v = v/Norm[v];
    support = Max[v.Wn];
    qeps = Min[qeps, support],
    {nDirections}
  ];
  qeps
];

(* Example usage (placeholder geometry) *)
pList = {
  {0.05, 0., 0.},
  {-0.025, 0.043301, 0.},
  {-0.025, -0.043301, 0.}
};

(* Simple frames with normals toward origin omitted for brevity *)
RList = Table[IdentityMatrix[3], {Length[pList]}];

muList = {0.8, 0.8, 0.8};
W = primitiveWrenches[pList, RList, muList, 8];

svals = SingularValueList[W];
qIso = Min[svals]/Max[svals];
qEps = epsilonSampled[W, 2000];

{qIso, qEps}
      

Mathematica can also construct ConvexHullMesh of the primitive wrenches (or a low-dimensional projection) and directly compute distances from the origin to the hull to approximate \(\epsilon\).

Problem 1 (Convexity of GWS). Let \(\mathcal{W}_0 = \{\mathbf{w}_1,\dots,\mathbf{w}_N\}\) be the set of primitive wrenches obtained from linearized friction cones. Show that the normalized GWS \(\mathcal{W}_1 = \operatorname{conv}(\mathcal{W}_0)\) is convex, and that the conic GWS \(\mathcal{W}_c = \operatorname{cone}(\mathcal{W}_0)\) is a convex cone.

Solution.

By definition, \[ \mathcal{W}_1 = \left\{ \sum_{k=1}^N \lambda_k \mathbf{w}_k \,\middle|\, \lambda_k \ge 0,\ \sum_{k=1}^N \lambda_k = 1 \right\}. \] Take any \(\mathbf{w},\mathbf{w}' \in \mathcal{W}_1\). Then \(\mathbf{w} = \sum_{k} \lambda_k \mathbf{w}_k\), \(\mathbf{w}' = \sum_{k} \lambda'_k \mathbf{w}_k\) with \(\lambda_k,\lambda'_k \ge 0\) and \(\sum_k \lambda_k = \sum_k \lambda'_k = 1\). For any \(t \in [0,1]\),

\[ t\mathbf{w} + (1-t)\mathbf{w}' = \sum_{k=1}^N \bigl(t\lambda_k + (1-t)\lambda'_k\bigr)\mathbf{w}_k. \]

The new coefficients satisfy \(t\lambda_k + (1-t)\lambda'_k \ge 0\) and \(\sum_k (t\lambda_k + (1-t)\lambda'_k) = t + (1-t) = 1\). Hence \(t\mathbf{w} + (1-t)\mathbf{w}' \in \mathcal{W}_1\), proving convexity.

For the conic GWS, \[ \mathcal{W}_c = \left\{ \sum_{k=1}^N \alpha_k \mathbf{w}_k \,\middle|\, \alpha_k \ge 0 \right\}, \] we check convexity and positive homogeneity. Let \(\mathbf{w} = \sum_k \alpha_k \mathbf{w}_k\), \(\mathbf{w}' = \sum_k \beta_k \mathbf{w}_k\) with \(\alpha_k,\beta_k \ge 0\). For any \(t \in [0,1]\),

\[ t\mathbf{w} + (1-t)\mathbf{w}' = \sum_{k=1}^N (t\alpha_k + (1-t)\beta_k)\mathbf{w}_k, \]

with \(t\alpha_k + (1-t)\beta_k \ge 0\), so \(t\mathbf{w} + (1-t)\mathbf{w}' \in \mathcal{W}_c\). For any \(s \ge 0\), \[ s\mathbf{w} = \sum_k (s\alpha_k)\mathbf{w}_k, \] with \(s\alpha_k \ge 0\), so \(s\mathbf{w} \in \mathcal{W}_c\). Thus \(\mathcal{W}_c\) is a convex cone.

Problem 2 (Force-Closure in 2D). Consider a planar rigid body (in \(\mathbb{R}^2\)) with two frictional point contacts at positions \(\mathbf{p}_1 = (1,0)\), \(\mathbf{p}_2 = (-1,0)\) in the body frame, with outward normals pointing along \(\pm x\)-axis, and identical friction coefficient \(\mu\). Assume we linearize each friction cone into two generators (top and bottom of the cone). Sketch the primitive wrenches in the planar wrench space \((f_x,f_y,\tau_z)\) and argue qualitatively when the grasp achieves force-closure.

Solution.

In 2D, a wrench is \(\mathbf{w} = (f_x,f_y,\tau_z)\). Each contact can generate a force roughly inside a cone centered along the inward normal. For \(\mathbf{p}_1 = (1,0)\), the inward normal points to the left (negative \(x\)), so the normal force is \((f_x,f_y) \approx (-f_n,0)\). Adding tangential components gives forces roughly pointing down-left and up-left, which are approximated by two generator forces \(\mathbf{f}_{1,1},\mathbf{f}_{1,2}\). Their wrenches are \(\mathbf{w}_{1,j} = (f_{x,1j}, f_{y,1j}, \tau_{z,1j})\) with \(\tau_{z,1j} = p_{1,x} f_{y,1j} - p_{1,y} f_{x,1j}\).

Similarly, at \(\mathbf{p}_2 = (-1,0)\), the inward normal points to the right, so the two generator forces point up-right and down-right, with corresponding wrenches \(\mathbf{w}_{2,1},\mathbf{w}_{2,2}\). The four primitive wrenches populate a roughly symmetric set in \((f_x,f_y,\tau_z)\)-space that expands as \(\mu\) grows.

For small \(\mu\), the tangential components are weak, so the wrenches lie almost along a 2D subspace and cannot span all wrench directions; the origin is not in the interior of their convex hull. As \(\mu\) increases, the tangential components increase, giving a 3D convex hull of primitive wrenches that can enclose the origin. Force-closure occurs once the origin lies in the interior of \(\operatorname{conv}\{\mathbf{w}_{i,j}\}\), which requires sufficiently large friction to generate wrenches that counter arbitrary planar forces and torques.

Problem 3 (Isotropy Metric and Singular Values). Let \(G \in \mathbb{R}^{6\times n}\) be a grasp map. Show that the isotropy metric \(Q_{\text{iso}} = \sigma_{\min}(G) / \sigma_{\max}(G)\) is invariant under right-multiplication of \(G\) by an orthogonal matrix \(Q \in \mathbb{R}^{n\times n}\) (i.e., \(Q^\top Q = I\)). Interpret this in terms of changing basis in the contact force space.

Solution.

The singular values of \(G\) are the square roots of the eigenvalues of \(G G^\top\). For any orthogonal \(Q\),

\[ (GQ)(GQ)^\top = G Q Q^\top G^\top = G G^\top. \]

Thus \(G\) and \(GQ\) have the same singular values, so \(\sigma_{\min}(GQ) = \sigma_{\min}(G)\) and \(\sigma_{\max}(GQ) = \sigma_{\max}(G)\). Therefore \(Q_{\text{iso}}(GQ) = Q_{\text{iso}}(G)\).

Geometrically, multiplying by an orthogonal matrix \(Q\) corresponds to a change of orthonormal basis in the contact force space \(\mathbb{R}^n\). Since this is just a rotation in the domain of the linear map \(G\), it does not change how “directionally uniform” the map is, hence the isotropy metric is invariant.

Problem 4 (\(\epsilon\)-Metric as Distance to Facets). Suppose the normalized GWS \(\mathcal{W}_1\) in \(\mathbb{R}^d\) is given in half-space form: \(\mathcal{W}_1 = \{\mathbf{w} \mid a_i^\top \mathbf{w} \le b_i,\ i=1,\dots,M\}\), with \(b_i > 0\) for all \(i\) and the origin in its interior. Show that with respect to the norm \(\|\mathbf{w}\|_{A} = \sqrt{\mathbf{w}^\top A \mathbf{w}}\), where \(A \succ 0\), the largest inscribed ball centered at the origin in \(\mathcal{W}_1\) has radius \[ Q_{\epsilon} = \min_{i=1,\dots,M} \frac{b_i}{\sqrt{a_i^\top A^{-1} a_i}}. \]

Solution.

The distance from the origin to the hyperplane \(a_i^\top \mathbf{w} = b_i\) under the norm induced by \(A\) can be computed by solving

\[ \min_{\mathbf{w}} \sqrt{\mathbf{w}^\top A \mathbf{w}} \quad \text{s.t.}\quad a_i^\top \mathbf{w} = b_i. \]

Using Lagrange multipliers, define \(\mathcal{L}(\mathbf{w},\lambda) = \mathbf{w}^\top A \mathbf{w} + \lambda(a_i^\top \mathbf{w} - b_i)\). Stationarity in \(\mathbf{w}\) gives \(2A\mathbf{w} + \lambda a_i = 0\), so \(\mathbf{w} = -\tfrac{\lambda}{2} A^{-1} a_i\). Enforcing the constraint \(a_i^\top \mathbf{w} = b_i\) yields

\[ a_i^\top\left(-\frac{\lambda}{2}A^{-1}a_i\right) = b_i \quad \Rightarrow\quad \lambda = -\frac{2b_i}{a_i^\top A^{-1} a_i}. \]

Substituting back, \(\mathbf{w}^* = \tfrac{b_i}{a_i^\top A^{-1} a_i} A^{-1} a_i\). The squared norm is

\[ \|\mathbf{w}^*\|_A^2 = (\mathbf{w}^*)^\top A \mathbf{w}^* = \left(\frac{b_i}{a_i^\top A^{-1} a_i}\right)^2 a_i^\top A^{-1} a_i = \frac{b_i^2}{a_i^\top A^{-1} a_i}, \]

hence the distance is \(b_i / \sqrt{a_i^\top A^{-1} a_i}\). The largest ball centered at the origin contained in \(\mathcal{W}_1\) is constrained by the nearest facet, so its radius is the minimum of these distances over all facets, which is the expression given for \(Q_{\epsilon}\).

Problem 5 (Relation Between \(\epsilon\) and Force-Closure). Show that \(Q_{\epsilon} > 0\) if and only if the origin lies in the interior of \(\mathcal{W}_1\).

Solution.

(\(\Rightarrow\)) If \(Q_{\epsilon} > 0\), then by definition there exists \(\epsilon > 0\) such that the closed ball \(B_2(\mathbf{0},\epsilon)\) is contained in \(\mathcal{W}_1\). Thus every small neighborhood of the origin (of radius up to \(\epsilon\)) lies in \(\mathcal{W}_1\), so the origin is an interior point.

(\(\Leftarrow\)) If the origin is in the interior, there exists some radius \(r > 0\) such that \(B_2(\mathbf{0},r) \subset \mathcal{W}_1\). Then any smaller radius \(0 < \epsilon \le r\) also satisfies \(B_2(\mathbf{0},\epsilon) \subset \mathcal{W}_1\), so the supremum over such \(\epsilon\) is strictly positive. Hence \(Q_{\epsilon} > 0\).

Combining this with the theorem in Section 4 yields the equivalence between positive \(\epsilon\)-metric and force-closure for grasps modeled with linearized friction cones.

In this lesson we defined the grasp wrench space as the set of all object wrenches achievable by admissible contact forces under friction and unilateral constraints. Starting from contact geometry and friction cones, we constructed primitive wrenches and used convex hulls and cones to characterize both normalized and scaled GWS. We linked force-closure to the inclusion of the origin in the interior of this space and studied several grasp quality metrics, including volume-based measures, matrix-based isotropy indices, and the Ferrari–Canny \(\epsilon\)-metric with its robustness interpretation. Finally, we provided multi-language implementations (Python, C++, Java, MATLAB/Simulink, Mathematica) that illustrate how to compute primitive wrenches and approximate grasp quality metrics in practical planning and analysis pipelines.