Kinematics, Contact Mechanics, and Grasp Quality#
The exporter preserves the link geometry used to generate contacts. The planner maps each resulting contact set to the forces and moments it can exert on the object, then ranks approach commands by their capacity under pose and friction uncertainty. This guide defines the kinematics, wrench scores, and lower-tail calculation. The planning guide provides the corresponding configuration and output fields.
Notation and Implementation#
Contact and load quantities use the object frame. Hand geometry uses the palm frame until transformed by the object-relative palm pose.
Symbol |
Meaning |
API or Record Correspondence |
|---|---|---|
\(q_i,q_i^0\) |
Joint angle and reference angle, radians |
Exported joint coordinates; |
\(u_j\) |
Independent driver coordinate, radians |
Exported DOF order; |
\(p_i,n_i\) |
Contact position in millimeters and inward unit normal |
|
\(c\) |
Object center of mass, millimeters |
Center argument of |
\(\mu\) |
Friction coefficient |
|
\(t_{i1},t_{i2},s_{ij}\) |
Tangent basis and polygon directions |
Constructed inside |
\(\rho\) |
Positive moment-normalization length, millimeters |
|
\(f_{ij},w_{ij}\) |
Edge force and normalized primitive wrench |
Edge forces define the wrench columns returned by |
\(G,W\) |
Primitive-wrench matrix and its convex hull |
Input to |
\(\varepsilon\) |
Ferrari–Canny radius |
|
\(p_k,f_k,\ell_k\) |
Task application point, balancing-force direction, and normalized load ray |
Rows of |
\(c_0,c_1\) |
Constant and linear coefficient of a joint equality |
MuJoCo equality coefficients; distinct from center of mass \(c\) |
\(f_i\) |
Physical contact force, with normal and tangential components |
Underlying contact model; |
\(Q\) |
Common supported task-load multiplier |
|
\(Z,\beta\) |
Quality reward and adverse-tail probability mass |
Flattened quality samples; |
Joint Coordinates and Link Geometry#
A hinge constrains the relative motion of two bodies to rotation about one axis. A serial finger chain composes these motions from the palm to the fingertip. Under the standard Denavit–Hartenberg convention, the transform from link frame \(i\) to its predecessor is
Here \(\theta_i^0\) is the DH angle at reference, \(d_i\) is the hinge-axis
offset, \(a_i\) is the common-normal length, and \(\gamma_i\) is the twist.
The exporter constructs DH frames from the compiled hinge axes at
reference and transforms collision meshes into their link frames.
Geometry rigidly attached to a link shares its transform. GraspIt! XML
stores angular constants in degrees and mesh coordinates in millimeters.
The Python link_poses API takes independent coordinates in radians and
returns palm-frame transforms indexed by (chain_index, link_index).
An affine MuJoCo equality can couple a driven hinge \(i\) to an independent hinge \(j\). For driver coordinate \(u_j=q_j\), the reference-coordinate constraint is
Its absolute-coordinate reconstruction is
For \(q_j^0=0.35\) rad, \(q_i^0=-0.20\) rad, \(c_0=0.16\) rad, and \(c_1=0.70\), the manifest offset is \(b_i=-0.285\) rad. The manifest stores the absolute-coordinate map in radians. The source convention is defined in the MuJoCo XML reference.
Frictional Point Contacts#
A contact can push inward and resist tangential slip through friction. For contact force \(f_i\in\mathbb R^3\), its normal component is \(f_{n,i}=n_i^\top f_i\) and its tangential component is \(f_{t,i}=f_i-f_{n,i}n_i\). The unilateral Coulomb conditions are
At each contact, the force has three components: one normal to the surface and two tangential components spanning the contact plane. The moment \((p_i-c)\times f_i\) about \(c\) is determined by this force and its lever arm. See Murray, Li, and Sastry, Chapter 5 and Rimon and Burdick, Chapter 4 for the grasp map and contact models.
Polygonal Wrench Construction#
By default, build_grasp_matrix approximates each friction cone with
eight edge forces. To construct their tangent directions, choose \(r_i=(0,0,1)\)
when \(|n_{i,z}|<0.9\) and \(r_i=(1,0,0)\) otherwise. An orthonormal
basis of the contact tangent plane is
For \(j=0,\ldots,7\), the directions are
These equations describe the ideal unit-vector construction; the code
adds a small denominator regularizer during normalization. Each edge
has unit normal-force component under the default
unit_normal_force=True:
Dividing moments by a length \(\rho>0\) gives the primitive wrench
The matrix \(G\) contains these columns. Its convex hull is the normalized grasp wrench space
Convex combinations assign nonnegative weights summing to one. Because
\(n_i^\top f_{ij}=1\), this imposes a unit total normal-force budget across
contacts. The unit budget refers to normal-force components. With
unit_normal_force=False, the builder additionally scales
them by \(1/\sqrt{1+\mu^2}\).
Force Closure and Ferrari–Canny Epsilon#
Force closure means that contacts can resist sufficiently small wrenches in every direction. Mathematically, the origin must lie in the interior of the full-dimensional hull \(W\). Write its facet representation as \(W=\{w:a_h^\top w\le b_h\ \forall h\}\), where \(h\) indexes facets. The Ferrari–Canny metric is
It is the radius of the largest origin-centered Euclidean ball contained in \(W\). For epsilon, the planner uses
This is a contact-derived radius. Epsilon comparisons depend on this
moment normalization, the friction coefficient, and the force budget;
see Roa and Suárez. compute_gws_epsilon(G)
returns (epsilon, is_force_closure). Its numerical hull checks can
return zero epsilon with a true Boolean at a boundary, so that Boolean
is not an exact strict-interior certificate. native_epsilon is the
separate simulator metric with its own normalization.
The min-weight API tests for an equilibrium combination with uniformly positive primitive weights. For \(G\) with \(M\) columns, it solves
min_weight_force_closure(G) returns (l_star, is_force_closure); its
Boolean requires both a margin above tol=1e-6 and matrix rank six.
The hull and LP computations require SciPy for these metrics; the
no-SciPy path uses a singular-value proxy.
Analytic Contact Example#
The following synthetic contact data place six contacts at the face centers of an axis-aligned cube, with inward normals and a contact radius of 25 mm. This analytic quality calculation uses prescribed contacts with NumPy and SciPy.
import numpy as np
from graspit_py.grasp.quality import (
build_grasp_matrix, compute_gws_epsilon, min_weight_force_closure,
)
positions_mm = 25.0 * np.vstack([np.eye(3), -np.eye(3)])
normals = -positions_mm / 25.0
center_mm = np.zeros(3)
G = build_grasp_matrix(positions_mm, normals, center_mm, friction_coef=0.5)
l_star, force_closure = min_weight_force_closure(G)
epsilon, _ = compute_gws_epsilon(G)
print('G shape:', G.shape)
print(f'min-weight: {l_star:.6f}; force closure: {force_closure}')
print(f'epsilon: {epsilon:.6f}')
Output:
G shape: (6, 48)
min-weight: 0.020833; force closure: True
epsilon: 0.408248
Task-Wrench Capacity#
A task score measures capacity along specified loading directions.
Each task_forces_mm row supplies an application point \(p_k\) and a
nonzero direction \(f_k\) for the contact force that balances the external
load. The planner normalizes \(\lVert f_k\rVert_2=1\) and constructs
For example, [0, 0, 0, 0, 0, 1] requests upward support through the
object-frame origin against downward gravity. Its moment about \(c\) is
zero only when that line of force passes through \(c\).
The task-wrench capacity is the largest common multiplier supported along every specified ray:
Using the facet convention above, this becomes
task_wrenches_from_forces constructs the normalized load rays;
compute_task_wrench_quality evaluates their common capacity against
the grasp matrix. Both functions are in graspit_py.grasp.quality.
The task scorer requires a full-dimensional hull with the origin strictly inside and returns zero if that check fails. \(Q\) multiplies the load ray under the unit total normal-force budget. Expressing it as a physical load in newtons requires a calibrated force budget.
Two-dimensional analogy for wrench capacity. The inscribed ball illustrates \(\varepsilon\); the task ray reaches the hull at \(Q\ell\). The planner computes its scores in six-dimensional wrench space.#
For task scoring the planner uses \(\rho=100\) mm for both the hull and the load rays. Applying the same invertible moment scaling to both preserves each ray-intersection multiplier. Epsilon depends on the scaling because it measures a Euclidean radius. For fixed contacts, increasing \(\mu\) expands the polygonal cone cross-sections and their hull, so the supported task multiplier is nondecreasing.
Lower-Tail Ranking#
A high mean capacity can coexist with low capacity in adverse poses or at low friction. The planner therefore averages the lowest quality values over a specified probability mass. For quality reward \(Z\) and tail mass \(\beta\in(0,1]\), lower-tail conditional value-at-risk is
Here \(\eta\) is a scalar threshold. lower_tail_mean(values, alpha=beta)
implements this reward measure; alpha is tail mass. The separate
cvar API averages the upper tail of costs, with
lower_tail_mean(q) = -cvar(-q) for matching arguments.
The settings n_pose_samples, pose_sigma_mm, and pose_sigma_deg
control the perturbed-pose count and the componentwise translation and
rotation-vector standard deviations. Each pose trial repeats the approach
and closure. Translation is added
in the object frame; a sampled rotation-vector rotation left-multiplies
the palm rotation and rotates the travel direction. The default is eight
perturbed poses plus the nominal pose, evaluated over nine equally
weighted friction coefficients. Failed perturbed approaches or closures
contribute a zero row. All pose–friction entries have equal weight,
including the nominal row.
Let \(z_{(1)}\le\cdots\le z_{(M)}\) be the sorted entries, \(m=\lfloor\beta M\rfloor\), and \(r=\beta M-m\). The empirical tail mean is
with the boundary term omitted when \(r=0\). The planner’s
tail_mass defaults to 0.2. Setting it to one gives the sample mean;
setting n_pose_samples=0 and supplying a single friction coefficient
gives nominal scoring at that coefficient. For nonnegative quality,
a zero-quality mass of at least \(\beta\) produces zero tail mean. See
Rockafellar and Uryasev for the discrete
probability treatment.
The fractional boundary matters when the tail mass does not select a whole number of samples. For scores \(0,0.03,0.10,0.11\) and \(\beta=0.375\), the tail contains the zero score and half the mass of the next score:
from graspit_py.risk import lower_tail_mean
value = lower_tail_mean([0.0, 0.03, 0.10, 0.11], alpha=0.375)
print(f'fractional empirical tail: {value:.6f}')
Output:
fractional empirical tail: 0.010000
The API also accepts normalized or unnormalized probability weights.
For values [0.0, 10.0], weights [0.1, 0.9], and alpha=0.5, it returns
8.0: the tail uses mass 0.1 from the first value and mass 0.4 from the
second, giving \((0.1\cdot0+0.4\cdot10)/0.5=8\).
FIRMGrasp evaluates the force-closure margin at
the mean of the adverse friction tail. Here the tail is taken over
quality values after closure simulation and friction-grid evaluation.
epsilon_cvar always stores the epsilon tail mean. With task loading,
score stores the tail mean of \(Q\); otherwise it equals epsilon_cvar.
The planning guide connects these quantities to complete configurations and returned grasp records. The API reference documents the Python operations, and the export guide describes the joint-coordinate map used to reconstruct a command.