In previous chapters you constructed candidate grasps using analytical criteria (force-closure, grasp wrench space) and sampling-based synthesis. In practice, execution errors, shape uncertainty, and unmodeled friction cause the executed grasp to deviate from the planned one. Tactile sensors provide rich local information that can be used to refine the grasp after initial contact.

Consider a multi-fingered hand with configuration \( q \in \mathbb{R}^n \), grasping a rigid object with \( m \) point or soft contacts. Stack all contact forces as \( \mathbf{f} = [\mathbf{f}_1^\top,\dots,\mathbf{f}_m^\top]^\top \in \mathbb{R}^{3m} \). The standard quasi-static contact model is

\[ \mathbf{w}_o + \mathbf{G}\mathbf{f} = \mathbf{0}, \]

where \( \mathbf{w}_o \in \mathbb{R}^6 \) is the net wrench on the object and \( \mathbf{G} \in \mathbb{R}^{6 \times 3m} \) is the grasp map constructed from individual contact wrenches (see Chapter 7).

For each contact \( j \in \{1,\dots,m\} \), let \( \mathbf{n}_j(q) \) be the outward contact normal, and decompose the contact force as

\[ f_{n,j} = \mathbf{n}_j(q)^\top \mathbf{f}_j, \quad \mathbf{f}_{t,j} = \mathbf{f}_j - f_{n,j}\,\mathbf{n}_j(q), \]

where \( f_{n,j} \) is the normal component and \( \mathbf{f}_{t,j} \) is the tangential component. Assuming Coulomb friction with coefficient \( \mu_j > 0 \), the friction cone constraint is

\[ \lVert \mathbf{f}_{t,j} \rVert_2 \leq \mu_j f_{n,j}. \]

Define the friction margin

\[ \gamma_j = \mu_j f_{n,j} - \lVert \mathbf{f}_{t,j} \rVert_2, \]

which is positive when the contact is safely inside the friction cone and becomes negative when impending slip is detected. Tactile-aware grasp refinement will use tactile measurements to estimate \( f_{n,j} \), \( \mathbf{f}_{t,j} \), and contact locations \( \mathbf{c}_j(q) \), and then adjust \( q \) to increase some notion of grasp robustness (e.g., the minimum \( \gamma_j \)).

Modern tactile sensors (e.g., pressure arrays, optical tactile sensors) provide a discretized tactile image on the fingertip surface. For the \( j \)-th fingertip, let \( \mathcal{I}_j \) denote the index set of tactile cells (taxels). Each taxel \( k \in \mathcal{I}_j \) has:

• Position \( \mathbf{p}_{j,k} \in \mathbb{R}^3 \) in the fingertip frame.
• Area \( a_k > 0 \).
• Measured normal pressure \( \sigma_{j,k} \geq 0 \).

Under a quasi-static assumption and negligible tangential deformation, a simple and widely used estimator for the resultant normal force and center of pressure is

\[ F_{n,j} = \sum_{k \in \mathcal{I}_j} \sigma_{j,k} a_k, \quad \mathbf{c}_j = \frac{1}{F_{n,j}} \sum_{k \in \mathcal{I}_j} \sigma_{j,k} a_k \mathbf{p}_{j,k}. \]

The fingertip normal \( \mathbf{n}_j(q) \) is typically known from the hand kinematics: it is the surface normal at the nominal contact point. For soft sensors that deform, small shifts in the pressure distribution encode tangential shear; one convenient approximation is to define a local contact frame so that the displacement of the center of pressure in the tangential plane is proportional to the tangential force:

\[ \mathbf{f}_{t,j} \approx \mathbf{K}_{s,j} \left( \mathbf{P}_T \left(\mathbf{c}_j - \mathbf{c}_j^{\text{rest}}\right) \right), \]

where \( \mathbf{K}_{s,j} \in \mathbb{R}^{2 \times 2} \) is an empirical shear stiffness matrix, \( \mathbf{P}_T \) projects into the tangent plane, and \( \mathbf{c}_j^{\text{rest}} \) is the rest (no-contact) center of pressure.

The tactile sensing model can be summarized as a (noisy) mapping

\[ \mathbf{y}_j = h_j(q,\mathbf{x}_o) + \boldsymbol{\eta}_j, \]

where \( \mathbf{y}_j \) is the stacked tactile observation on finger \( j \), \( \mathbf{x}_o \) is the object pose, and \( \boldsymbol{\eta}_j \) represents measurement noise and modeling errors. The refinement algorithm will only require local derivatives of the estimated contact features with respect to \( q \):

\[ \mathbf{c}_j(q + \Delta q) \approx \mathbf{c}_j(q) + \mathbf{J}_{c_j}(q)\,\Delta q, \quad \mathbf{n}_j(q + \Delta q) \approx \mathbf{n}_j(q) + \mathbf{J}_{n_j}(q)\,\Delta q, \]

where \( \mathbf{J}_{c_j}(q), \mathbf{J}_{n_j}(q) \in \mathbb{R}^{3 \times n} \) can be obtained by differentiating the forward kinematics plus the fingertip geometry with respect to the joint coordinates.

Let us construct a differentiable local objective that we will minimize with respect to \( q \). For each contact \( j \), define a slip indicator

\[ h_j(q) = \frac{\lVert \mathbf{f}_{t,j}(q) \rVert_2}{\mu_j f_{n,j}(q)} - 1. \]

Note that \( h_j(q) \leq 0 \) if and only if the contact lies inside the friction cone; \( h_j(q) > 0 \) signals that the estimated force is outside the cone and slip is occurring or imminent. A smooth penalty for near-slip behavior is

\[ \phi_j(q) = \max\bigl(0, h_j(q)\bigr)^2, \]

which is differentiable almost everywhere (subgradients exist at \( h_j(q) = 0 \)). In addition, tactile localization provides estimates \( \mathbf{c}_j(q) \) and \( \mathbf{n}_j(q) \) that can be compared to desired contact locations \( \mathbf{c}_j^{\text{des}} \) and normals \( \mathbf{n}_j^{\text{des}} \) from the nominal planned grasp.

We define the global cost

\[ J(q) = \sum_{j=1}^{m} \Bigl( w_{\text{slip}}\,\phi_j(q) + w_{\text{pos}}\, \lVert \mathbf{c}_j(q) - \mathbf{c}_j^{\text{des}} \rVert_2^2 + w_{\text{align}}\, \lVert \mathbf{n}_j(q) - \mathbf{n}_j^{\text{des}} \rVert_2^2 \Bigr), \]

where \( w_{\text{slip}}, w_{\text{pos}}, w_{\text{align}} > 0 \) are design parameters. Minimizing \( J(q) \) attempts to:

• Push contacts back inside their friction cones.
• Align the observed contacts with their planned positions.
• Align fingertip normals with desired surface normals.

Locally, we approximate \( J(q) \) around the current configuration \( q_k \) using first-order expansions of the features \( \mathbf{c}_j, \mathbf{n}_j, h_j \). For example, the position term for contact \( j \) becomes

\[ \begin{aligned} \lVert \mathbf{c}_j(q_k + \Delta q) - \mathbf{c}_j^{\text{des}} \rVert_2^2 &\approx \left\lVert \mathbf{c}_j(q_k) + \mathbf{J}_{c_j}(q_k)\Delta q - \mathbf{c}_j^{\text{des}} \right\rVert_2^2 \\ &= \Delta q^\top \mathbf{J}_{c_j}(q_k)^\top \mathbf{J}_{c_j}(q_k) \Delta q + 2\bigl(\mathbf{c}_j(q_k) - \mathbf{c}_j^{\text{des}}\bigr)^\top \mathbf{J}_{c_j}(q_k)\Delta q + \text{const}. \end{aligned} \]

A similar expansion holds for the normal alignment term, with \( \mathbf{J}_{n_j}(q_k) \) replacing \( \mathbf{J}_{c_j}(q_k) \).

Collecting the quadratic terms from all contacts, the local approximation of \( J(q_k + \Delta q) \) takes the form

\[ \widetilde{J}(\Delta q) = \tfrac{1}{2}\Delta q^\top H \Delta q + g^\top \Delta q + \text{const}, \]

where, ignoring the slip term for simplicity,

\[ \begin{aligned} H &= 2\sum_{j=1}^{m} \Bigl( w_{\text{pos}}\, \mathbf{J}_{c_j}(q_k)^\top \mathbf{J}_{c_j}(q_k) + w_{\text{align}}\, \mathbf{J}_{n_j}(q_k)^\top \mathbf{J}_{n_j}(q_k) \Bigr) + \lambda I_n, \\ g^\top &= 2\sum_{j=1}^{m} \Bigl( w_{\text{pos}}\, \bigl(\mathbf{c}_j(q_k) - \mathbf{c}_j^{\text{des}}\bigr)^\top \mathbf{J}_{c_j}(q_k) + w_{\text{align}}\, \bigl(\mathbf{n}_j(q_k) - \mathbf{n}_j^{\text{des}}\bigr)^\top \mathbf{J}_{n_j}(q_k) \Bigr), \end{aligned} \]

and \( \lambda > 0 \) is a regularization parameter. The slip penalties \( \phi_j(q) \) can be approximated linearly in \( h_j(q) \) to yield additional terms in \( g \) and possibly \( H \).

We also enforce linear inequality constraints on \( \Delta q \) derived from:

• Joint limits: \( q_{\min} \leq q_k + \Delta q \leq q_{\max} \).
• Linearized slip margins: \( h_j(q_k) + \nabla_q h_j(q_k)^\top \Delta q \leq 0 \).

Overall, the tactile-aware refinement step at iteration \( k \) is the solution of the quadratic program (QP)

\[ \begin{aligned} \min_{\Delta q \in \mathbb{R}^n} \quad & \tfrac{1}{2}\Delta q^\top H \Delta q + g^\top \Delta q \\ \text{s.t.} \quad & A \Delta q \leq b, \end{aligned} \]

where \( A \) and \( b \) encode the linear constraints above.

Proposition (Positive definiteness of \( H \)). Suppose \( w_{\text{pos}}, w_{\text{align}}, \lambda > 0 \), and let \( \mathcal{J} \) denote the set of contacts used in \( J(q) \). If the stacked Jacobian

\[ \mathbf{J}_{\text{stack}} = \begin{bmatrix} \vdots \\ \mathbf{J}_{c_j}(q_k) \\ \mathbf{J}_{n_j}(q_k) \\ \vdots \end{bmatrix} \]

has full column rank, then \( H \) is symmetric positive definite, and the QP has a unique minimizer.

Sketch of proof. Each summand \( \mathbf{J}_{c_j}^\top \mathbf{J}_{c_j} \) and \( \mathbf{J}_{n_j}^\top \mathbf{J}_{n_j} \) is symmetric positive semidefinite. If \( \mathbf{J}_{\text{stack}} \) has full column rank, then for any nonzero \( \Delta q \) at least one term contributes a strictly positive quadratic form, implying that the sum is positive definite. Adding \( \lambda I_n \) (with \( \lambda > 0 \)) preserves positive definiteness. Hence \( H \) is positive definite and the unconstrained minimizer is unique; feasibility of the constraints guarantees uniqueness of the constrained minimizer.

After preplanning a grasp, the tactile-aware refinement operates as a closed-loop procedure: execute a preliminary closing motion, read tactile data, solve the local QP for \( \Delta q \), update joints, and repeat until convergence or a maximum number of iterations is reached.

flowchart TD
  S["Initial planned grasp q0"] --> C1["Close hand to make contact"]
  C1 --> M1["Read tactile arrays yj on each finger"]
  M1 --> E1["Estimate contact pose cj, normal nj, forces fj"]
  E1 --> QP["Form local quadratic program for delta q"]
  QP --> SOLVE["Solve optimization and obtain delta q"]
  SOLVE --> UPD["Update joints: q = q + delta q"]
  UPD --> CHECK{"Converged (cost change small)"}
  CHECK -->|"no"| M1
  CHECK -->|"yes"| DONE["Execute task with refined grasp"]
  

In practice, convergence can be monitored by the decrease in \( J(q) \), by the norm \( \lVert \Delta q \rVert_2 \), or by the minimum friction margin \( \min_j \gamma_j \).

We now outline a Python implementation of a gradient-based refinement step. For simplicity, we approximate the QP solution with one projected gradient step. In practice you would use cvxpy, osqp, or qpsolvers to solve the full QP. Kinematic quantities such as the fingertip Jacobians can be obtained from libraries like roboticstoolbox or simulators such as PyBullet.

import numpy as np

class TactileContact:
    def __init__(self, positions, pressures, areas,
                 c_des, n_des, mu):
        """
        positions: (K,3) array of taxel positions p_{j,k}
        pressures: (K,) array of pressures sigma_{j,k}
        areas:     (K,) array of taxel areas a_k
        c_des:     (3,) desired contact position
        n_des:     (3,) desired surface normal (unit)
        mu:        scalar friction coefficient
        """
        self.positions = np.asarray(positions, dtype=float)
        self.pressures = np.asarray(pressures, dtype=float)
        self.areas = np.asarray(areas, dtype=float)
        self.c_des = np.asarray(c_des, dtype=float)
        self.n_des = np.asarray(n_des, dtype=float)
        self.mu = float(mu)

def center_of_pressure(contact):
    w = contact.pressures * contact.areas
    F_n = np.sum(w) + 1e-9  # avoid division by zero
    c = (w[:, None] * contact.positions).sum(axis=0) / F_n
    return F_n, c

def slip_indicator(F_n, f_t_norm, mu):
    # h_j = ||f_t|| / (mu F_n) - 1
    return f_t_norm / (mu * F_n + 1e-9) - 1.0

def tactile_grasp_refine_step(q, contacts, Jc_fn, Jn_fn,
                              w_slip=1.0, w_pos=1.0, w_align=0.1,
                              step_size=1e-2,
                              q_min=None, q_max=None):
    """
    q:        (n,) current joint configuration
    contacts: list of TactileContact objects, one per fingertip
    Jc_fn:    function (q, j) -> (3,n) Jacobian of contact position
    Jn_fn:    function (q, j) -> (3,n) Jacobian of contact normal
    """
    q = np.asarray(q, dtype=float)
    n = q.size
    grad = np.zeros_like(q)

    for j, contact in enumerate(contacts):
        F_n_est, c_est = center_of_pressure(contact)

        # For illustration we use a crude proxy for tangential force norm
        # as the magnitude of deviation from the desired contact location.
        pos_err = c_est - contact.c_des
        f_t_norm_est = np.linalg.norm(pos_err)

        h = slip_indicator(F_n_est, f_t_norm_est, contact.mu)
        slip_penalty = max(0.0, h)

        Jc = Jc_fn(q, j)  # shape (3,n)
        Jn = Jn_fn(q, j)  # shape (3,n)

        # Position gradient: d/dq ||c(q) - c_des||^2 = 2 Jc^T (c - c_des)
        grad_pos = 2.0 * Jc.T @ pos_err

        # Normal alignment (assuming we can estimate surface normal from kinematics)
        n_est = np.zeros(3,)  # placeholder; use geometric model
        n_err = n_est - contact.n_des
        grad_align = 2.0 * Jn.T @ n_err

        # Slip penalty gradient (simple heuristic)
        # d/dq max(0, h(q))^2 ≈ 2 max(0,h) * (dh/dq)
        # Here we approximate dh/dq via Jc and normalize.
        if slip_penalty > 0.0:
            if np.linalg.norm(pos_err) > 1e-6:
                dir_vec = pos_err / np.linalg.norm(pos_err)
                dh_dq = (Jc.T @ dir_vec) / (contact.mu * F_n_est + 1e-9)
            else:
                dh_dq = np.zeros_like(q)
            grad_slip = 2.0 * slip_penalty * dh_dq
        else:
            grad_slip = np.zeros_like(q)

        grad += (w_pos * grad_pos
                 + w_align * grad_align
                 + w_slip * grad_slip)

    # Gradient descent step
    dq = -step_size * grad
    q_new = q + dq

    # Project onto joint limits if provided
    if q_min is not None:
        q_new = np.maximum(q_new, np.asarray(q_min))
    if q_max is not None:
        q_new = np.minimum(q_new, np.asarray(q_max))

    return q_new
      

In a simulation environment you would call tactile_grasp_refine_step in a loop as tactile data arrives, updating the robot's joint targets after each step.

In C++, we can use Eigen to implement the same gradient step. The following code assumes that kinematics and fingertip geometry are available from a separate robotics library.

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

struct TactileContact {
    Eigen::MatrixXd positions;  // K x 3
    Eigen::VectorXd pressures;  // K
    Eigen::VectorXd areas;      // K
    Eigen::Vector3d c_des;
    Eigen::Vector3d n_des;
    double mu;
};

static inline void center_of_pressure(
    const TactileContact& c,
    double& F_n,
    Eigen::Vector3d& c_est)
{
    Eigen::VectorXd w = c.pressures.cwiseProduct(c.areas);
    F_n = w.sum() + 1e-9;
    Eigen::Vector3d num = c.positions.transpose() * w;
    c_est = num / F_n;
}

static inline double slip_indicator(double F_n,
                                    double f_t_norm,
                                    double mu)
{
    return f_t_norm / (mu * F_n + 1e-9) - 1.0;
}

// User-provided Jacobian callbacks
typedef Eigen::MatrixXd (*JacobianFn)(const Eigen::VectorXd& q, int contact_index);

Eigen::VectorXd tactileGraspRefineStep(
    const Eigen::VectorXd& q,
    const std::vector<TactileContact>& contacts,
    JacobianFn Jc_fn,
    JacobianFn Jn_fn,
    double w_slip  = 1.0,
    double w_pos   = 1.0,
    double w_align = 0.1,
    double step_size = 1e-2,
    const Eigen::VectorXd* q_min = nullptr,
    const Eigen::VectorXd* q_max = nullptr)
{
    Eigen::VectorXd grad = Eigen::VectorXd::Zero(q.size());

    for (std::size_t j = 0; j < contacts.size(); ++j) {
        const auto& c = contacts[j];
        double F_n;
        Eigen::Vector3d c_est;
        center_of_pressure(c, F_n, c_est);

        Eigen::Vector3d pos_err = c_est - c.c_des;
        double f_t_norm_est = pos_err.norm();

        double h = slip_indicator(F_n, f_t_norm_est, c.mu);
        double slip_penalty = std::max(0.0, h);

        Eigen::MatrixXd Jc = Jc_fn(q, static_cast<int>(j)); // 3 x n
        Eigen::MatrixXd Jn = Jn_fn(q, static_cast<int>(j)); // 3 x n

        Eigen::VectorXd grad_pos =
            2.0 * Jc.transpose() * pos_err;

        Eigen::Vector3d n_est = Eigen::Vector3d::Zero(); // placeholder
        Eigen::Vector3d n_err = n_est - c.n_des;
        Eigen::VectorXd grad_align =
            2.0 * Jn.transpose() * n_err;

        Eigen::VectorXd grad_slip = Eigen::VectorXd::Zero(q.size());
        if (slip_penalty > 0.0) {
            if (pos_err.norm() > 1e-6) {
                Eigen::Vector3d dir_vec = pos_err.normalized();
                Eigen::VectorXd dh_dq =
                    Jc.transpose() * dir_vec / (c.mu * F_n + 1e-9);
                grad_slip = 2.0 * slip_penalty * dh_dq;
            }
        }

        grad += w_pos * grad_pos
              + w_align * grad_align
              + w_slip * grad_slip;
    }

    Eigen::VectorXd dq = -step_size * grad;
    Eigen::VectorXd q_new = q + dq;

    if (q_min) {
        q_new = q_new.cwiseMax(*q_min);
    }
    if (q_max) {
        q_new = q_new.cwiseMin(*q_max);
    }
    return q_new;
}
      

This function can be called inside a real-time loop that also exchanges tactile data with the robot hardware.

In Java, we can implement the refinement as an object that operates on primitive arrays. For linear algebra we can either rely on plain arrays or a library such as EJML.

import java.util.List;

class TactileContact {
    double[][] positions; // K x 3
    double[] pressures;   // K
    double[] areas;       // K
    double[] cDes;        // length 3
    double[] nDes;        // length 3
    double mu;
}

interface JacobianProvider {
    // Return 3 x n Jacobian matrix in row-major order
    double[][] Jc(double[] q, int contactIndex);
    double[][] Jn(double[] q, int contactIndex);
}

public class TactileGraspRefiner {
    private final double wSlip;
    private final double wPos;
    private final double wAlign;
    private final double stepSize;

    public TactileGraspRefiner(double wSlip,
                               double wPos,
                               double wAlign,
                               double stepSize) {
        this.wSlip = wSlip;
        this.wPos = wPos;
        this.wAlign = wAlign;
        this.stepSize = stepSize;
    }

    private static double[] centerOfPressure(TactileContact c) {
        int K = c.pressures.length;
        double[] result = new double[4]; // [F_n, cx, cy, cz]
        double F = 0.0;
        double cx = 0.0, cy = 0.0, cz = 0.0;
        for (int k = 0; k < K; ++k) {
            double w = c.pressures[k] * c.areas[k];
            F += w;
            cx += w * c.positions[k][0];
            cy += w * c.positions[k][1];
            cz += w * c.positions[k][2];
        }
        F += 1e-9;
        result[0] = F;
        result[1] = cx / F;
        result[2] = cy / F;
        result[3] = cz / F;
        return result;
    }

    private static double slipIndicator(double F_n,
                                        double fTNorm,
                                        double mu) {
        return fTNorm / (mu * F_n + 1e-9) - 1.0;
    }

    public double[] refineOnce(double[] q,
                               List contacts,
                               JacobianProvider jac,
                               double[] qMin,
                               double[] qMax) {
        int n = q.length;
        double[] grad = new double[n];

        for (int j = 0; j < contacts.size(); ++j) {
            TactileContact c = (TactileContact) contacts.get(j);
            double[] cop = centerOfPressure(c);
            double F_n = cop[0];
            double[] cEst = new double[]{cop[1], cop[2], cop[3]};

            double posNorm = 0.0;
            double[] posErr = new double[3];
            for (int i = 0; i < 3; ++i) {
                posErr[i] = cEst[i] - c.cDes[i];
                posNorm += posErr[i] * posErr[i];
            }
            posNorm = Math.sqrt(posNorm);
            double h = slipIndicator(F_n, posNorm, c.mu);
            double slipPenalty = Math.max(0.0, h);

            double[][] Jc = jac.Jc(q, j);
            double[][] Jn = jac.Jn(q, j);

            double[] gradPos = new double[n];
            for (int col = 0; col < n; ++col) {
                double acc = 0.0;
                for (int row = 0; row < 3; ++row) {
                    acc += Jc[row][col] * posErr[row];
                }
                gradPos[col] = 2.0 * acc;
            }

            double[] nEst = new double[]{0.0, 0.0, 0.0};
            double[] nErr = new double[3];
            for (int i = 0; i < 3; ++i) {
                nErr[i] = nEst[i] - c.nDes[i];
            }
            double[] gradAlign = new double[n];
            for (int col = 0; col < n; ++col) {
                double acc = 0.0;
                for (int row = 0; row < 3; ++row) {
                    acc += Jn[row][col] * nErr[row];
                }
                gradAlign[col] = 2.0 * acc;
            }

            double[] gradSlip = new double[n];
            if (slipPenalty > 0.0 && posNorm > 1e-6) {
                double[] dir = new double[3];
                for (int i = 0; i < 3; ++i) {
                    dir[i] = posErr[i] / posNorm;
                }
                for (int col = 0; col < n; ++col) {
                    double acc = 0.0;
                    for (int row = 0; row < 3; ++row) {
                        acc += Jc[row][col] * dir[row];
                    }
                    gradSlip[col] = 2.0 * slipPenalty *
                                    acc / (c.mu * F_n + 1e-9);
                }
            }

            for (int i = 0; i < n; ++i) {
                grad[i] += wPos * gradPos[i]
                         + wAlign * gradAlign[i]
                         + wSlip * gradSlip[i];
            }
        }

        double[] qNew = new double[n];
        for (int i = 0; i < n; ++i) {
            double dq = -stepSize * grad[i];
            double qi = q[i] + dq;
            if (qMin != null) {
                qi = Math.max(qMin[i], qi);
            }
            if (qMax != null) {
                qi = Math.min(qMax[i], qi);
            }
            qNew[i] = qi;
        }
        return qNew;
    }
}
      

This code is intentionally simple; in a full implementation you would replace the placeholder normal estimation with geometric computations and couple this class to a robot middleware such as ROS.

MATLAB is well suited for prototyping tactile-aware refinement and then embedding the logic as a MATLAB Function block in Simulink for closed-loop simulation with a hand–object dynamics model.

function q_new = tactileGraspRefineStep(q, contacts, Jc_fn, Jn_fn, ...
    w_slip, w_pos, w_align, step_size, q_min, q_max)
%TACTILEGRASPREFINESTEP One gradient-based refinement step.
%
%   q:        n x 1 joint vector
%   contacts: struct array with fields positions (Kx3),
%             pressures (Kx1), areas (Kx1),
%             c_des (1x3), n_des (1x3), mu (scalar)
%   Jc_fn:    function handle @(q,j) -> 3 x n contact Jacobian
%   Jn_fn:    function handle @(q,j) -> 3 x n normal Jacobian

if nargin < 5 || isempty(w_slip),  w_slip  = 1.0;  end
if nargin < 6 || isempty(w_pos),   w_pos   = 1.0;  end
if nargin < 7 || isempty(w_align), w_align = 0.1;  end
if nargin < 8 || isempty(step_size), step_size = 1e-2; end

n = numel(q);
grad = zeros(n,1);

for j = 1:numel(contacts)
    c = contacts(j);
    w = c.pressures(:) .* c.areas(:);
    F_n = sum(w) + 1e-9;
    num = c.positions.' * w;
    c_est = num / F_n;

    pos_err = c_est(:) - c.c_des(:);
    f_t_norm_est = norm(pos_err);
    h = f_t_norm_est / (c.mu * F_n + 1e-9) - 1.0;
    slip_penalty = max(0.0, h);

    Jc = Jc_fn(q, j); % 3 x n
    Jn = Jn_fn(q, j); % 3 x n

    grad_pos = 2.0 * Jc.' * pos_err;

    n_est = zeros(3,1); % placeholder
    n_err = n_est - c.n_des(:);
    grad_align = 2.0 * Jn.' * n_err;

    grad_slip = zeros(n,1);
    if slip_penalty > 0.0 && f_t_norm_est > 1e-6
        dir_vec = pos_err / f_t_norm_est;
        dh_dq = (Jc.' * dir_vec) / (c.mu * F_n + 1e-9);
        grad_slip = 2.0 * slip_penalty * dh_dq;
    end

    grad = grad + w_pos * grad_pos + ...
                 w_align * grad_align + ...
                 w_slip * grad_slip;
end

dq = -step_size * grad;
q_new = q(:) + dq;

if nargin >= 9 && ~isempty(q_min)
    q_new = max(q_new, q_min(:));
end
if nargin >= 10 && ~isempty(q_max)
    q_new = min(q_new, q_max(:));
end
      

To integrate this into Simulink, wrap tactileGraspRefineStep in a MATLAB Function block and feed joint states, tactile arrays, and kinematic Jacobians from other blocks corresponding to the hand model and tactile sensor emulator.

Mathematica is convenient for symbolic derivation of gradients and for exploring the behavior of local cost functions before implementing them in real-time code. The example below considers a single fingertip contact with a simple quadratic cost.

(* Symbolic variables *)
n = 2; (* number of joints in toy example *)
q = Array[q, n];
cDes = {c1, c2, c3};
nDes = {n1, n2, n3};
wPos = Symbol["wPos"];
wAlign = Symbol["wAlign"];

(* Local linear models for contact position and normal *)
Jc = Array[Jc, {3, n}];
Jn = Array[Jn, {3, n}];
c0 = {c0x, c0y, c0z};
n0 = {n0x, n0y, n0z};

c[q_] := c0 + Jc . q;
nHat[q_] := n0 + Jn . q;

posErr[q_] := c[q] - cDes;
nErr[q_] := nHat[q] - nDes;

Jlocal[q_] :=
  wPos*posErr[q].posErr[q] +
  wAlign*nErr[q].nErr[q];

gradJ = Grad[Jlocal[q], q];
hessJ = D[gradJ, {q}];

(* Inspect symbolic expressions *)
gradJ // Simplify
hessJ // Simplify

(* Numeric example *)
params = {
  wPos -> 1.0,
  wAlign -> 0.2,
  c1 -> 0.0, c2 -> 0.0, c3 -> 0.0,
  n1 -> 0.0, n2 -> 0.0, n3 -> 1.0,
  c0x -> 0.01, c0y -> -0.02, c0z -> 0.98,
  n0x -> 0.0, n0y -> 0.0, n0z -> 1.0
};

JcNum = { {0.01,0.0},{0.0,0.02},{-0.01,0.01} };
JnNum = { {0.0,0.0},{0.0,0.0},{0.0,0.0} };

gradJFun = Function[{q1, q2},
  gradJ /. params /. {
    q[1] -> q1, q[2] -> q2,
    Jc -> JcNum,
    Jn -> JnNum
  }
];

(* One gradient descent step from an initial q *)
qInit = {0.0, 0.0};
stepSize = 0.05;
dq = gradJFun[qInit[[1]], qInit[[2]]];
qNew = qInit - stepSize*dq
      

Symbolically obtained gradients can be exported and reused in embedded code for performance-critical implementations.

Problem 1 (Center of pressure as a weighted least-squares estimator). For a single fingertip with taxels \( k \in \mathcal{I} \), define the cost

\[ E(\mathbf{p}) = \sum_{k \in \mathcal{I}} \sigma_k a_k \lVert \mathbf{p} - \mathbf{p}_k \rVert_2^2, \]

where \( \sigma_k a_k \) is the normal force contribution of taxel \( k \). Show that the minimizer \( \mathbf{p}^\star \) equals the center of pressure formula given in Section 2.

Solution. Expand the squared norm:

\[ \lVert \mathbf{p} - \mathbf{p}_k \rVert_2^2 = \mathbf{p}^\top \mathbf{p} - 2 \mathbf{p}^\top \mathbf{p}_k + \mathbf{p}_k^\top \mathbf{p}_k. \]

Thus

\[ E(\mathbf{p}) = \left(\sum_k \sigma_k a_k\right)\mathbf{p}^\top \mathbf{p} - 2 \mathbf{p}^\top \left(\sum_k \sigma_k a_k \mathbf{p}_k\right) + \text{const}. \]

Differentiating with respect to \( \mathbf{p} \) and setting the gradient to zero:

\[ \nabla_{\mathbf{p}} E(\mathbf{p}) = 2\left(\sum_k \sigma_k a_k\right) \mathbf{p} - 2 \sum_k \sigma_k a_k \mathbf{p}_k = \mathbf{0}, \]

which yields

\[ \mathbf{p}^\star = \frac{1}{\sum_k \sigma_k a_k} \sum_k \sigma_k a_k \mathbf{p}_k, \]

exactly the center of pressure expression used earlier.

Problem 2 (Convexity and uniqueness of the local QP). Consider the QP in Section 4 without the slip term and with \( \lambda > 0 \). Prove that the objective is strictly convex and that the QP admits a unique minimizer whenever the feasible set is nonempty.

Solution. The Hessian of the quadratic objective is \( H \), given by

\[ H = 2\sum_{j} w_{\text{pos}}\, \mathbf{J}_{c_j}^\top \mathbf{J}_{c_j} + 2\sum_j w_{\text{align}}\, \mathbf{J}_{n_j}^\top \mathbf{J}_{n_j} + \lambda I_n. \]

Each matrix \( \mathbf{J}_{c_j}^\top \mathbf{J}_{c_j} \) and \( \mathbf{J}_{n_j}^\top \mathbf{J}_{n_j} \) is symmetric positive semidefinite. With \( w_{\text{pos}}, w_{\text{align}} \geq 0 \) and \( \lambda > 0 \), for any nonzero \( \Delta q \):

\[ \Delta q^\top H \Delta q = 2\sum_j w_{\text{pos}} \lVert \mathbf{J}_{c_j} \Delta q \rVert_2^2 + 2\sum_j w_{\text{align}} \lVert \mathbf{J}_{n_j} \Delta q \rVert_2^2 + \lambda \lVert \Delta q \rVert_2^2 > 0. \]

Hence \( H \) is symmetric positive definite, so the quadratic form is strictly convex. A strictly convex function on a convex feasible set has at most one minimizer. Since the inequality constraints define a convex polyhedron and we assume it is nonempty, the QP has a unique solution.

Problem 3 (First-order decrease with gradient step). Let \( J(q) \) be differentiable with Lipschitz continuous gradient (with constant \( L \)) in a neighborhood of \( q_k \). Show that the unconstrained gradient step \( q_{k+1} = q_k - \alpha \nabla J(q_k) \) satisfies

\[ J(q_{k+1}) \leq J(q_k) - \alpha \left(1 - \frac{L \alpha}{2}\right) \lVert \nabla J(q_k) \rVert_2^2 \]

for all \( \alpha \in (0, 2/L] \). Conclude that small steps guarantee a decrease in \( J \).

Solution. The standard descent lemma for \( L \)-smooth functions states:

\[ J(q_k + s) \leq J(q_k) + \nabla J(q_k)^\top s + \frac{L}{2}\lVert s \rVert_2^2. \]

Substitute \( s = -\alpha \nabla J(q_k) \):

\[ J(q_{k+1}) \leq J(q_k) - \alpha \lVert \nabla J(q_k) \rVert_2^2 + \frac{L \alpha^2}{2} \lVert \nabla J(q_k) \rVert_2^2, \]

which simplifies to the claimed inequality. For \( 0 < \alpha \leq 2/L \), the coefficient \( 1 - L\alpha/2 \) is nonnegative, so the right-hand side does not exceed \( J(q_k) \). Therefore, \( J(q_{k+1}) \leq J(q_k) \), with strict decrease whenever \( \nabla J(q_k) \neq \mathbf{0} \) and \( 0 < \alpha < 2/L \).

Problem 4 (Friction margin from tactile estimates). Suppose tactile processing yields estimates \( f_{n,j} = 5\,\text{N} \), \( \lVert \mathbf{f}_{t,j} \rVert_2 = 2\,\text{N} \), and \( \mu_j = 0.5 \) at contact \( j \). Compute the friction margin \( \gamma_j \) and the slip indicator \( h_j \). Decide whether the contact is safely inside the friction cone.

Solution. The friction margin is

\[ \gamma_j = \mu_j f_{n,j} - \lVert \mathbf{f}_{t,j} \rVert_2 = 0.5 \cdot 5 - 2 = 2.5 - 2 = 0.5\,\text{N}. \]

The slip indicator is

\[ h_j = \frac{\lVert \mathbf{f}_{t,j} \rVert_2}{\mu_j f_{n,j}} - 1 = \frac{2}{2.5} - 1 = 0.8 - 1 = -0.2. \]

Since \( \gamma_j > 0 \) and \( h_j < 0 \), the contact lies inside the friction cone with a nonzero safety margin.

Problem 5 (Decision flow between open-loop and tactile refinement). In an industrial setting, tactile sensing can be noisy and slow. Design a logical decision flow that decides whether to run refinement (solve the QP) on a given grasp, or accept the open-loop planned grasp without refinement. Argue which measurements or metrics should drive this decision.

Solution (conceptual flow). One possible decision flow is shown below. A key principle is to trigger refinement only when tactile cues indicate that the realized grasp deviates meaningfully from the planned grasp or that friction margins are low.

flowchart TD
  ST["Planned grasp executed"] --> TC["Read tactile and estimate contacts and margins"]
  TC --> DEV{"Deviation or low margin"}
  DEV -->|"no (stable)"| ACC["Accept open-loop grasp and proceed"]
  DEV -->|"yes (refine)"| REF["Run tactile-aware refinement step"]
  REF --> MON["Monitor convergence and safety metrics"]
  MON --> DONE["Use refined grasp for task"]
  

Here, the decision node compares estimated contact positions and normals against their planned values, and monitors the minimum friction margin \( \min_j \gamma_j \). Tactile refinement is only run when necessary, trading off robustness against computation and sensor delays.

In this lesson we formalized tactile-aware grasp refinement as a local optimization problem over the joint configuration \( q \). Starting from tactile measurements on each fingertip, we derived estimators for contact location and friction margins, defined a differentiable cost \( J(q) \) that encodes slip risk and geometric deviations from the planned grasp, and constructed a local quadratic approximation leading to a constrained QP in \( \Delta q \).

We showed that under mild assumptions the QP is strictly convex and admits a unique solution, and we provided language-agnostic implementations (Python, C++, Java, MATLAB/Simulink, Mathematica) to integrate tactile refinement into existing manipulation stacks. In the next lesson, you will use these ideas inside a full grasp planning pipeline in simulation, combining high-level grasp synthesis with low-level tactile adjustments.