Lesson 2.3.3 — Contact-Rich Manipulation

Week 14

Prerequisite lessons: 2.3.1 (grasp synthesis and quality), 2.3.2 (motion planning for manipulation), 2.2.4 (offline-to-online fine-tuning), 2.1.5 (visual SLAM and odometry), 1.1.3 (PID, computed torque, trajectory tracking), 1.1.4 (the full-stack pick-and-place pipeline).

Prior results used in this lesson. From 1.1.3: the computed-torque law $\tau = M(q)(\ddot q_d + K_d \dot e + K_p e) + C \dot q + g(q)$ and the fact that it was derived with zero external force. From 2.3.1: force closure and the Ferrari–Canny $\varepsilon$ grasp-quality metric. From 2.3.2: a collision-free path with a clearance margin. From 2.2.4: the runtime Q-shield and the deploy–collect–fine-tune loop. From 2.1.5: the eye-in-hand relative-pose estimator and the kinematic prior $T_W^C(q) = T_W^E(q) T_E^C$. Each is restated at the point of use.

What this lesson hands forward. The contact model, the impedance/admittance control law, and the residual-learning architecture that Lesson 3.1.4 (VLA fine-tuning) extends to pre-trained policies, and that Module 4.2 (building the full-stack agent) uses as the policy layer beneath the reasoning layer.

The running case in one sentence. The same 6-DOF arm carries the same parallel-jaw gripper over the same table; it must grasp the 5 cm, 0.1 kg cube at $T_W^{Obj} = (R_z(\pi/4), [0.5, 0.2, 0.3])$ with friction coefficient $\mu = 0.5$, carry it past the divider at $x = 0.35$, and release it into the bin at $[0.4, -0.3, 0.15]$ — and every one of those steps is a contact event.

§L.0 — Lesson map, objectives, and misconceptions

Observable outcomes. By the end of this lesson you can, without notes: (1) derive the steady-state position error of the 1.1.3 controller under a constant contact wrench; (2) state the Signorini complementarity and Coulomb friction conditions; (3) compute the minimum grip force for a misaligned grasp; (4) explain why contact stiffness makes high-gain position control unstable; (5) write the torque-level impedance control law and compute the rendered Cartesian stiffness; (6) choose impedance or admittance control from the environment stiffness; (7) describe hybrid position/force control and its selection matrix; (8) design a residual-RL policy on top of an impedance controller with a clipped action; (9) specify domain-randomization ranges from identification uncertainty; (10) budget the grasp force across the pick-and-place and design a contact-detection and recovery sequence.

Misconception inventory.

Everything is self-contained. Where a prerequisite lesson is used, the result is restated at the point of use.

§L.0.5 — Retrieval warm-up

Q1. In 1.1.3, the computed-torque law cancels the nonlinear terms $C\dot q + g(q)$ and imposes a linear error dynamics. Under what assumption was that derivation made?

Answer. The derivation assumed no external force at the end-effector, so the plant was $M\ddot q + C\dot q + g = \tau$. Used in §L.A.1 to show that the assumption is exactly what contact violates.

Q2. In 2.3.1, a grasp is force-closed if every external wrench can be resisted by contact forces inside the friction cones. What quantity does the $\varepsilon$ metric measure?

Answer. The radius of the largest ball centered at the origin of the grasp wrench space that fits inside the set of achievable wrenches under the linearized friction cones (Ferrari & Canny 1992, tier A, S). Used in §L.B.1 to connect force closure to a computable number.

Q3. In 2.3.2, a planned path carries a clearance margin. What does the margin protect against, and what invalidates it?

Answer. It protects against model error and control tracking error; it is invalidated when the payload’s actual swept volume exceeds the planned one, for example when the object slips or swings in the grasp. Used in §L.E.1 to explain why transport swing threatens the divider clearance.

Q4. In 2.1.5, the eye-in-hand camera’s pose is available from forward kinematics through $T_W^C(q) = T_W^E(q) T_E^C$. What does that estimate not give during a grasp?

Answer. It gives the gripper pose but not the object pose: if the object slips in the grasp, the camera moves with the gripper while the object does not. Used in §L.E.2 to motivate visual object tracking during contact.

Part A — Why contact breaks the position-control pipeline

§L.A.1 — The plant changes when contact begins

The controller from 1.1.3 was derived from the manipulator dynamics with no external force:

When the end-effector touches the environment, an external wrench $F_{ext}$ acts at the contact point and enters through the Jacobian transpose:

This is the only change, and it invalidates the 1.1.3 derivation. Substitute the computed-torque law $\tau = M(q)(\ddot q_d + K_d \dot e + K_p e) + C\dot q + g(q)$ with $e = q_d - q$:

Since $\ddot e = \ddot q_d - \ddot q$,

The right-hand side is the contact wrench mapped into joint space and scaled by the inverse inertia. At steady state with constant $F_{ext}$, $\dot e = \ddot e = 0$, so

The contact force appears as a position error. The controller has no way to distinguish “the arm is being pushed” from “the arm is behind its reference.” Its only mechanism for generating force is to be displaced, so it settles at the displacement that produces exactly the force the environment demands. This is the algebraic statement of the first misconception: contact is not a disturbance to be rejected, it is a new equilibrium.

The effective Cartesian stiffness of the robot is $K_x = K_p M$ in the simplest case. In series with the contact stiffness $k_c$, the force produced by a commanded penetration $d$ is

So the robot’s own stiffness sets the force, not the contact’s. This is the seed of impedance control: if the robot’s rendered stiffness determines the force, then rendering a chosen stiffness is a way to choose the force.

Self-explanation 1. In one sentence, why is the steady-state position error proportional to the contact force?

Answer. Because the controller’s only mechanism for generating force against the environment is a position error acting through the robot’s stiffness $K_x = K_p M$.

Self-explanation 2. Why does a high-gain position controller against a stiff environment become unstable rather than accurate?

Answer. Raising $K_p$ raises the loop gain and pushes the contact poles toward the contact natural frequency $\omega_c = \sqrt{k_c/m}$; once the controller bandwidth approaches $\omega_c$, sampling delay erases the phase margin.

§L.A.2 — Impacts, friction, and the four failure modes of position control

Three further properties of contact defeat position control.

Impacts are open-loop. When the end-effector first touches a surface with normal velocity $v_n^-$, the velocity jumps to $v_n^+ = -e\, v_n^-$ for coefficient of restitution $e$. The impulse is delivered over a duration of order the contact’s natural period, typically well under a millisecond for stiff materials. A controller sampling at 1 kHz sees the impact as a single-sample event; it cannot modulate the force. The only defence is to arrive with low normal velocity.

Friction is set-valued. At zero tangential velocity, the friction force may be any vector inside the friction cone; the Coulomb model does not specify which. This non-uniqueness breaks the differentiability that gradient-based learning and linear control both rely on. The maximum-dissipation principle selects a unique force during sliding, but not during sticking.

The plant is hybrid. Free motion, sticking contact, sliding contact, and impact are four distinct dynamical modes. A controller designed for one mode is not automatically valid in another. Robustness requires either a controller that is safe in all modes (impedance control, Part C) or explicit mode detection and switching (Part E).

Concept check. A learner adds integral action to the 1.1.3 controller to eliminate the steady-state error of Eq. (A.2). What happens to the contact force, and why is the fix wrong?

Answer. The integrator sees a persistent position error — the environment holds the arm back — and keeps increasing the command until the force saturates or the object deforms. The steady-state position error goes to zero, but the force becomes whatever the environment demands, which can be arbitrarily large. The mechanism is integrator windup against a stiff constraint. [displaces M1]

Worked-example ladder.

WE-L.A.a (fully worked). Take $M = I$, $K_p = 100 I$, $J = I$, and a constant contact force $F_{ext} = [5, 0, 0]^\top$ N. Compute the steady-state error and the robot’s Cartesian stiffness.

Solution. From Eq. (A.2), $e_{ss} = -100^{-1} \cdot 5 = -0.05$ m along $x$. The robot must be displaced 5 cm to generate 5 N. Its Cartesian stiffness is $K_x = K_p M = 100$ N/m. To generate the same 5 N with a stiffer loop, $K_p = 10{,}000$, the displacement would be 0.5 mm — but see the next rung.

WE-L.A.b (completion). With $K_p = 10{,}000$, $K_x = 10{,}000$ N/m and the contact stiffness is $k_c = 10^5$ N/m. Compute the effective series stiffness from Eq. (A.3) and the contact natural frequency for a 1 kg effective mass. Is the loop margin adequate?

Completion. $K_{eff} = (1/10^4 + 1/10^5)^{-1} = 9{,}091$ N/m. The contact natural frequency is $\omega_c = \sqrt{(K_x + k_c)/m} = \sqrt{1.1 \times 10^5} \approx 332$ rad/s. The controller bandwidth is roughly $\sqrt{K_x/m} \approx 100$ rad/s, within a factor of 3.3 of $\omega_c$; with 1 kHz sampling and a few milliseconds of delay, the phase margin at crossover is thin. The lesson: reducing the 5 cm error to 0.5 mm by raising gain buys accuracy at the cost of stability.

WE-L.A.c (faded). Derive the closed-loop characteristic equation for the 1-DOF case with contact spring $k_c$, controller stiffness $K_x$, and a pure delay $T_d$. Faded step: the loop is $m s^2 = -K_x(e^{-s T_d} - 1) - k_c$ about the contact equilibrium; linearize the delay as $1 - s T_d$ and find the critical $K_x$. The completion shows that the critical gain falls as $1/T_d$, so faster sampling buys stability margin. The lesson: contact stability is a sampling-rate problem as much as a gain problem.

WE-L.A.d (contrastive near-miss). A learner observes that the arm chatters at about 300 Hz against the cube and concludes that the force sensor is noisy. Replacing the sensor does not help. Why it fails: the chatter frequency matches $\omega_c/2\pi \approx 53$ Hz for $k_c = 10^5$ and $m = 1$… the observed 300 Hz corresponds to a stiffer contact, $k_c \approx 3.5 \times 10^6$ N/m, consistent with the metal fingers contacting the cube’s hard plastic corners rather than the rubber pads. The fix: lower $K_p$ until the loop bandwidth is at least a factor of five below $\omega_c$, or switch to impedance control. The near-miss teaches that chatter frequency is a diagnostic for contact stiffness, not for sensor noise.

Comparative table: control modes in contact.

Design principle P33.1. Regulate mechanical impedance, not position, whenever contact is expected. Position control treats force as an error to be eliminated; impedance control treats force and motion as a relation to be shaped.

Consolidation checkpoint 1

(a) Three capabilities. You can derive Eq. (A.2) and interpret it. You can compute the series stiffness of robot and contact. You can diagnose chatter frequency as a contact-stiffness signature.

(b) Transfer probe. A 2 kg payload is held by a position-controlled arm with $K_x = 5000$ N/m against a spring-loaded fixture of stiffness $2 \times 10^4$ N/m. What steady-state force does a 2 mm commanded penetration produce? Answer: $K_{eff} = (1/5000 + 1/20000)^{-1} = 4000$ N/m, so $F = 8$ N.

(c) Remediation rule. If you cannot explain why the steady-state error is proportional to force, re-derive Eq. (A.2) before Part B.

Part B — Contact models and the friction cone

§L.B.1 — Rigid contact: complementarity, Coulomb friction, force closure

A rigid contact between the finger and the cube is described by three conditions at each contact point.

Non-penetration and non-adhesion (Signorini). Let $\phi(q) \ge 0$ be the gap between the bodies. Then

where $\lambda_n$ is the normal force. The three conditions say: bodies do not interpenetrate, contact can only push, and force is nonzero only when the gap is zero. The product condition is a complementarity constraint; it makes the contact a hybrid system by construction.

Coulomb friction. The tangential force $\lambda_t$ satisfies

and during sliding it opposes the slip velocity with maximum magnitude: $\lambda_t = -\mu \lambda_n \, v_t / |v_t|$. Equation (B.2) defines the friction cone: the set of admissible contact forces is a cone with apex at the contact point, half-angle $\arctan \mu$.

Force closure. Restating the result of 2.3.1: stack the contact forces into $\lambda$ and form the grasp matrix $G$ so that the object wrench is $w = G\lambda$. The grasp is force-closed if for every wrench $w$ there exists $\lambda$ inside the friction cones with $G\lambda = -w$. The $\varepsilon$ metric of Ferrari & Canny (1992, tier A, S) is the radius of the largest ball centered at the origin of the wrench space that fits inside the achievable set; it is computed as a linear program once the cones are linearized.

Linearizing the cone. Replace the disc $|\lambda_t| \le \mu \lambda_n$ with a polygon of $m$ edges with directions $d_i$:

The linearization turns force-closure testing and grasp-force optimization into linear programs, at the cost of a conservative inner approximation (Anitescu & Potra 1997, tier A, S).

Self-explanation 3. Why is the set of admissible contact forces a cone rather than a bounded set?

Answer. Because the tangential limit scales with the normal force, which is unbounded above; scaling any admissible force by a positive factor keeps it admissible.

§L.B.2 — Soft contact models and the stiffness scale

Simulators rarely solve the complementarity conditions exactly. MuJoCo and most physics engines use a soft contact model in which the normal force is a function of penetration. The simplest is the penalty model

and the energy-consistent variant of Hunt & Crossley (1975, tier A, S) replaces the linear damping with a penetration-dependent term. The penalty model is simple but imposes a timestep constraint: the simulation must resolve the contact natural frequency, so $\Delta t \ll 2/\omega_c$ with $\omega_c = \sqrt{k_c/m}$. For $k_c = 10^5$ N/m and $m = 0.1$ kg, $\omega_c \approx 1000$ rad/s and the timestep must be well below 2 ms; MuJoCo defaults to 2 ms with implicit integration that tolerates stiffer contacts than an explicit integrator would.

The stiffness scale matters for control. At a 5 N grip force with $k_c = 10^5$ N/m, the penetration is $\delta = F/k_c = 50\ \mu$m. That is below the repeatability of most arms and comparable to the resolution of a good force sensor. The encoder cannot see the contact; the force sensor can. This is the practical reason force sensing, or force estimation, is mandatory for contact-rich work.

Worked-example ladder.

WE-L.B.a (fully worked). The cube is rotated by $R_z(\pi/4)$, so its grasped faces have normals at $45^\circ$ to the world axes. The gripper closes along world $y$, so the finger force $F$ has normal component $F\cos\theta$ and tangential component $F\sin\theta$ at the contact, with $\theta = 45^\circ$. Compute the minimum finger force to hold the cube at rest, with $m = 0.1$ kg, $\mu = 0.5$, and two fingers.

Solution. The available friction at each contact is $\mu F \cos\theta$. Two contacts must carry the weight: $2 \mu F \cos\theta \ge mg$. So

The standard 5 N grip force gives a margin of $5/1.39 \approx 3.6$.

WE-L.B.b (completion). Include a transport acceleration $a = 2$ m/s² along the direction that adds to gravity. Compute the required finger force and the margin at 5 N.

Completion. Replace $g$ with $g + a = 11.81$ m/s²: $F \ge 0.1 \cdot 11.81 / 0.7071 = 1.67$ N. Margin at 5 N is $5/1.67 = 3.0$. The completion shows that the acceleration term costs about 20% of the margin at $a = 2$ m/s².

WE-L.B.c (faded). Linearize the friction cone with four edges and write the force-closure linear program for the planar two-contact grasp. Faded step: the constraints are $G\lambda = -w$, $\beta_i \ge 0$, $\sum \beta_i \le \mu \lambda_n$ at each contact. The completion shows that the planar grasp is force-closed iff the line joining the two contacts lies inside the friction cone at both contacts — the “friction-cone-of-the-line” condition. For $\theta = 45^\circ$, $\mu = 0.5$, the half-angle is $\arctan 0.5 = 26.6^\circ < 45^\circ$, so the rotated grasp is not force-closed against arbitrary wrenches in the plane; it is closed only against wrenches whose direction lies within the cone. This is the precise reason the rotated grasp is weaker than the aligned one.

WE-L.B.d (contrastive near-miss). A learner computes the grip force from $\mu$ alone (taking $\theta = 0$) and obtains 0.98 N; in experiment the cube slips at 1.2 N. The discrepancy is attributed to “noisy friction.” Why it fails: the actual contact is at the cube’s corners rather than the face centres, so the effective $\theta$ is larger than $45^\circ$ and there is an additional moment arm about the grasp axis. The real requirement is closer to 1.7 N, matching the observed slip at 1.2 N only when the corner geometry and the moment are included. The fix: use the contact geometry from the grasp planner (2.3.1) rather than assuming face-centre contact, or instrument the fingers with tactile sensing to measure the actual contact patch.

Comparative table: contact models.

Design principle P33.2. Model the contact you cannot measure; measure the contact you cannot model. Use the friction cone and force closure for planning and force budgets; use force or tactile sensing for the residual that the model misses.

Consolidation checkpoint 2

(a) Three capabilities. You can state the Signorini conditions and the Coulomb cone. You can compute the minimum grip force for a misaligned grasp. You can compute penetration depth from force and stiffness and argue why the encoder cannot resolve it.

(b) Transfer probe. The cube’s mass doubles to 0.2 kg and $\mu$ drops to 0.4. What grip force is required at rest with the $45^\circ$ grasp? Answer: $F \ge 2 \cdot 0.981 / (2 \cdot 0.4 \cdot 0.7071) = 3.47$ N, within the 5 N standard but with margin only 1.4.

(c) Remediation rule. If you cannot derive Eq. (B.5), re-derive it from the two-contact force balance before Part C.

Part C — Impedance and admittance control

§L.C.1 — Impedance control: shaping the motion–force relation

Impedance control (Hogan 1985, tier A, S) does not regulate position or force; it regulates the relation between them. The target behaviour at the end-effector is

where $x$ is the Cartesian position, $x_d$ the reference, and $M_d, B_d, K_d$ the desired inertia, damping, and stiffness. Against a stiff environment, the impedance behaves like a spring of stiffness $K_d$; against a soft environment, it behaves like a mass–damper.

The torque-level implementation solves Eq. (C.1) for the required acceleration and applies inverse dynamics:

With a force sensor, $F_{ext}$ is measured and the law is exact. Without one, the last term is dropped and the rendered impedance is approximate; the robot’s own inertia adds to $M_d$.

A simpler joint-space variant is

which renders a Cartesian stiffness $K_x \approx J^{-\top} K_q J^{-1}$ near the operating configuration. It is the workhorse for contact-rich manipulation because it requires no force sensor and is trivially safe: the force is bounded by $K_x$ times the maximum position error.

Self-explanation 1. Why does impedance control render a stiffness rather than a force?

Answer. Because the rendered relation is $F = K_d \Delta x$ in the static limit; the controller chooses $K_d$ and lets the environment determine the force through the displacement.

§L.C.2 — Admittance control and the stiffness-matching rule

Admittance control inverts the causality. It measures the external force and integrates the admittance

to produce a reference motion $x_d$, which a position controller tracks. The robot appears to the environment as an admittance: force in, motion out.

The choice between the two is governed by the stiffness-matching rule. If the environment stiffness $k_e$ is much larger than the robot’s achievable stiffness $K_x$, use admittance: against a stiff wall, a small position error produces a large force, and the admittance integrates that force into motion, which is stable. If $k_e \ll K_x$, use impedance: the environment is compliant, and the robot can render the desired stiffness directly. The rule is a heuristic, not a theorem — the full stability condition is the passivity of the two-port formed by robot and environment — but it holds whenever the stiffnesses dominate the dynamics.

Hybrid position/force control (Raibert & Craig 1981, tier A, S) takes a third route: decompose the task space with a selection matrix $S$ and control $S x$ in position while controlling $(I - S) F$ in force. The decomposition must respect the task’s natural constraints: along a surface, the normal direction is force-controlled and the tangential directions are position-controlled. The selection matrix is a modelling choice, and a wrong choice produces fighting between the two loops.

Force estimation without a sensor. The momentum observer (De Luca & Mattone 2005, tier A, S) computes

where $p = M(q)\dot q$ is the generalized momentum. The residual $r$ converges to $J^\top F_{ext}$. It needs only joint torque (measured or inferred from motor current) and the dynamic model, and it is the mechanism by which a sensorless arm can detect contact.

Concept check. A learner implements force control as a position loop whose reference is $x_d + F_{err}/K_p$. Why does this fail against an unknown environment?

Answer. The gain $K_p$ is the robot’s stiffness, not the environment’s; the loop regulates position, and the force is whatever the environment produces at that position. Against a stiff environment the effective force gain is enormous and the loop is unstable; against a soft environment it is tiny and the force error persists. The mechanism is that force is not the controlled variable. [displaces M3]

Worked-example ladder.

WE-L.C.a (fully worked). A 2-DOF planar arm has Jacobian $J = 0.2 I$ at the operating configuration and joint stiffness $K_q = 100 I$ N·m/rad. Compute the Cartesian stiffness and the deflection under a 5 N contact force.

Solution. $K_x = J^{-\top} K_q J^{-1} = 100 / 0.04 = 2500$ N/m. Deflection $\Delta x = F/K_x = 5/2500 = 2$ mm. The contact stiffness $k_c = 10^5$ N/m is 40 times larger, so the robot is the compliant element and impedance control is in its stable regime.

WE-L.C.b (completion). Raise $K_q$ to $1000 I$ to reduce the deflection. Compute the new $K_x$, the deflection, and the ratio $K_x/k_c$. Is the loop still comfortably stable?

Completion. $K_x = 25{,}000$ N/m, deflection 0.2 mm, $K_x/k_c = 0.25$. The contact natural frequency is $\sqrt{(K_x + k_c)/m} \approx \sqrt{1.25 \times 10^5} \approx 354$ rad/s for $m=1$; the controller bandwidth is $\sqrt{25{,}000} = 158$ rad/s, only a factor of 2.2 below. With delay, the margin is thin. The completion shows the trade-off: reducing deflection by raising stiffness moves toward instability.

WE-L.C.c (faded). Analyse the steady state of the admittance (C.4) under a constant force. Faded step: set derivatives to zero: $K_d(x_d - x_0) = F_{ext}$, so $x_d = x_0 + F_{ext}/K_d$. With $K_d = 0$ the equation is $\ddot x_d = F_{ext}/M_d$ and the reference drifts — this is force control, not admittance. The completion shows the design trade-off: $K_d \to 0$ gives force control, $K_d \to \infty$ gives stiff position control, and the useful admittance sits between.

WE-L.C.d (contrastive near-miss). A learner uses impedance control with $K_d = 5000$ N/m against a steel plate with $k_e = 10^6$ N/m and observes 150 Hz chatter. The learner lowers the loop gain of the inner position loop and the chatter persists. Why it fails: the chatter is the contact mode $\sqrt{(K_d + k_e)/m} \approx 1000$ rad/s = 159 Hz, not the position loop. Lowering the position gain does not move it; only reducing $K_d$ or switching to admittance does. The fix: with a stiff environment, use admittance control (or reduce $K_d$ below 1000 N/m). The near-miss teaches that the relevant stiffness in the stability analysis is the sum of robot and environment stiffness.

Comparative table: impedance, admittance, hybrid force/position.

Design principle P33.3. Choose impedance or admittance by comparing the environment stiffness to the robot’s achievable stiffness; choose hybrid control only when the constraint directions are known.

Consolidation checkpoint 3

(a) Three capabilities. You can write the torque-level impedance law and compute $K_x$ from $K_q$. You can analyse the admittance steady state. You can apply the stiffness-matching rule.

(b) Transfer probe. A robot with $K_x = 1000$ N/m must wipe a window whose glass stiffness is $10^7$ N/m. Which scheme, and why? Answer: admittance, because the environment is four orders of magnitude stiffer than the robot; an impedance controller would need to render a stiffness below 1000 N/m to avoid instability, and the force would be uncontrolled.

(c) Remediation rule. If you cannot state why the chatter frequency is $\sqrt{(K_d + k_e)/m}$, re-derive the contact mode before Part D.

Part D — Learning contact-rich manipulation

§L.D.1 — Why learn, and what to learn

The contact model of Part B has parameters that are poorly identified: friction coefficient, contact stiffness, damping, and actuator delay each carry 20–50% uncertainty in practice. Model-based impedance control is robust to this uncertainty but cannot adapt the grasp force to the object or the task. Learning can.

The architecture that has proven most reliable is residual reinforcement learning (Johannink et al. 2019, tier B, S): a base controller provides a safe nominal action, and a learned policy outputs a correction,

where $o$ is the observation and $\pi_\theta$ is trained by any off-policy algorithm from Module 2.2. The base controller bounds the behaviour; the residual learns the part the model gets wrong. The action space is a design choice with consequences.

Action spaces. Torque actions are expressive but high-dimensional and unsafe; a small torque error produces a large force error against a stiff contact. Cartesian velocity actions are safe but cannot regulate contact force. Impedance-parameter actions — the policy outputs $K_d$ and $B_d$, or a target pose for the impedance — are the sweet spot for contact-rich work: they are low-dimensional, bounded, and physically meaningful. For the running case, the natural action is a residual on the impedance setpoint $\Delta x$, clipped to a safe bound.

Tactile sensing. GelSight (Yuan et al. 2017, tier A, S) measures contact geometry at sub-millimetre resolution; a wrist force/torque sensor measures the aggregate wrench. Both are observations the policy can use. For the running case, the wrist wrench plus the momentum-observer residual of Eq. (C.5) is sufficient.

§L.D.2 — Sim-to-real for contact

The contact sim-to-real gap is dominated by four parameters: friction coefficient, contact stiffness, actuator delay, and object mass. Domain randomization (Tobin et al. 2017, tier A, S) samples these from distributions during training so the policy does not overfit the nominal. Dactyl (OpenAI et al. 2018, tier A, S) demonstrated that large-scale randomization of these parameters, combined with a recurrent policy and distributed training, can produce dexterous in-hand manipulation that transfers to a real hand.

The ranges matter. They should reflect the identification uncertainty, not arbitrary numbers: if system identification (2.1.4 for geometry, Part B here for dynamics) bounds $\mu$ to $[0.3, 0.7]$, randomize over that interval. Wider randomization makes the policy more conservative and slower; narrower randomization leaves a gap.

Self-explanation 2. Why does residual RL preserve safety better than end-to-end RL?

Answer. Because the base controller bounds the action, and the residual is clipped to a perturbation whose worst-case force $K_d \delta_{max}$ is known; end-to-end RL has no such bound.

Self-explanation 3. Why does the randomization range for friction need to be at least as wide as the identification uncertainty?

Answer. Because a policy trained on a narrower range will exploit the training distribution; if the real friction lies outside, the policy’s grip-force choice is systematically wrong.

Worked-example ladder.

WE-L.D.a (fully worked). The residual policy outputs $\Delta x$ added to the impedance setpoint. With $K_d = 1000$ N/m and a safe force limit $F_{safe} = 20$ N, compute the clipping bound and the force produced if the raw residual is 5 cm.

Solution. $\delta_{max} = F_{safe}/K_d = 20/1000 = 2$ cm. The raw 5 cm is clipped to 2 cm, producing at most 20 N. The clipping bound is the safety mechanism.

WE-L.D.b (completion). Set the domain-randomization ranges for the running case given identification uncertainty: $\mu = 0.5 \pm 0.2$, $k_c = 10^5 \pm 0.5 \times 10^5$ N/m, delay $= 10 \pm 10$ ms, mass $= 0.1 \pm 0.02$ kg. State the ranges and justify each.

Completion. $\mu \in [0.3, 0.7]$, $k_c \in [5 \times 10^4, 1.5 \times 10^5]$ N/m, delay $\in [0, 20]$ ms, mass $\in [0.08, 0.12]$ kg. Each range is the identification interval, not wider; widening beyond it costs performance without buying robustness.

WE-L.D.c (faded). Derive the optimal behaviour under the reward $r = -|x_{obj} - x_{goal}| - \lambda F_{peak} - \gamma \mathbf{1}[\text{slip}]$. Faded step: take the expectation over the randomization distribution and differentiate with respect to the grip-force policy. The completion shows that large $\lambda$ yields gentle, slow placement and low slip; small $\lambda$ yields fast placement and occasional slip.

WE-L.D.d (contrastive near-miss). A learner trains with nominal $\mu = 0.5$ and deploys on a slightly damp cube with $\mu = 0.35$. The cube slips during transport. Why it fails: no randomization; the policy learned a grip force just above the nominal requirement. The fix: randomize $\mu$ over the identification range and train with the slip penalty active. The near-miss is the contact sim-to-real gap in miniature.

Comparative table: action spaces for contact-rich learning.

Design principle P33.4. Learn the residual, not the whole policy, when a safe controller exists; clip the residual to a force-bounded perturbation.

Consolidation checkpoint 4

(a) Three capabilities. You can write the residual action and its clip. You can set randomization ranges from identification uncertainty. You can state the reward trade-off between speed and force.

(b) Transfer probe. A peg-insertion policy is trained with $\mu \in [0.2, 0.8]$ and deployed on a peg with $\mu = 0.1$. What happens? Answer: the policy’s lateral force is calibrated for at least 0.2, so at 0.1 the peg jams; the policy should have been trained with a wider lower bound if $\mu = 0.1$ is plausible.

(c) Remediation rule. If you cannot justify each randomization range from an identification interval, re-read §L.D.2 before Part E.

Part E — The running case: budgeting, detection, recovery

§L.E.1 — The contact budget of the pick-and-place

The pick-and-place has four contact events: grasp, lift, transport, and release. Each has a force budget.

Grasp. The cube is at $R_z(\pi/4)$, so the grasp is misaligned by $45^\circ$. From Eq. (B.5), the minimum finger force at rest is 1.39 N; with a 2 m/s² transport acceleration it is 1.67 N. The standard 5 N grip gives a 3× margin. The maximum acceleration before slip at 5 N is

about 2.6 g. The nominal transport acceleration should stay below half of that, i.e., 12 m/s², to leave margin for friction variation.

Transport past the divider. The nominal straight path from $[0.5, 0.2, 0.3]$ to $[0.4, -0.3, 0.15]$ crosses the divider’s $y$-range at $x \approx 0.47$–$0.49$, which is 12–14 cm clear of the divider at $x = 0.35$. The gripper’s outer width is 8 cm, so the gripper’s trailing edge has at least 4 cm clearance. The threat is not the nominal path but swing: if the cube rotates in the grasp, its corner can move several centimetres. The rotation is resisted by the friction moment at the contacts; the maximum rotation before slip is set by the friction cone of the contact patch. For a 5 N grip and a 2.5 cm half-width, the resisting moment is roughly $2 \mu F \cdot 0.025 \approx 0.125$ N·m, while the inertial moment at 2 m/s² is $m a \cdot 0.025 \approx 0.005$ N·m; the margin is large. Swing is therefore not the dominant risk; the dominant risk is the release.

Release. Dropping the cube from $z = 0.3$ to the bin bottom at $z = 0.15$ is a 15 cm fall. Impact velocity is $\sqrt{2g \cdot 0.15} = 1.72$ m/s. With a restitution of 0.3, the cube rebounds at 0.52 m/s and rises 1.4 cm — not enough to leave the bin, but enough to be deflected by the rim if the cube has lateral velocity at release. The fix is to descend to 1 cm above the bin bottom with a minimum-jerk profile ending at zero velocity, so the cube is released at rest.

§L.E.2 — Detection, recovery, and fusion with 2.1.5

Detection. The momentum observer of Eq. (C.5) produces a residual $r$ that converges to $J^\top F_{ext}$. A contact is declared when $|r|$ exceeds a threshold for three consecutive control cycles. At 1 kHz, the latency is 3 ms, well below the tens of milliseconds over which slip develops. The threshold is set from the model uncertainty: if the dynamic model is accurate to 2 Nm at the wrist, the threshold is 4 Nm.

Recovery. The state machine from 1.1.4 gains a contact-recovery state: stop the trajectory, back off along the estimated contact normal by 1 cm, re-plan with the 2.3.2 planner, and retry. The health checks of 1.1.4 gain a contact-force check at the control boundary and an object-pose-consistency check at the perception boundary.

Fusion with 2.1.5. During contact, forward kinematics gives the gripper pose $T_W^E(q)$ but not the cube pose: if the cube slips, the gripper and the cube diverge. The eye-in-hand camera tracks the cube with the 2.1.2 6D pose estimator; if the depth channel fails on the cube’s shiny surface, the 2.1.5 visual odometry maintains the camera pose, and the kinematic prior $T_W^C(q) = T_W^E(q) T_E^C$ cross-checks it. The cube pose estimate is therefore maintained through the grasp, and a slip is detected as a divergence between the camera-tracked cube pose and the gripper pose.

Self-explanation 1. Why does the rotated grasp reduce the safety margin even though the friction coefficient is unchanged?

Answer. Because the finger force’s normal component is $F\cos\theta$, so the available friction $\mu F\cos\theta$ is reduced by the cosine of the misalignment.

Self-explanation 2. Why is a 3 ms contact-detection latency acceptable while a 50 ms latency is not?

Answer. Slip velocity under a 1 g tangential overload is about 1 m/s; in 3 ms the cube moves 3 mm, recoverable; in 50 ms it moves 5 cm, past the point of no return.

Worked-example ladder.

WE-L.E.a (fully worked). Compute the maximum transport acceleration at the standard 5 N grip, and the minimum grip force to survive $a = 2$ m/s².

Solution. From Eq. (E.1), $a_{max} = 25.5$ m/s². For $a = 2$: $F \ge m(g+a)/(2\mu\cos\theta) = 0.1 \cdot 11.81/0.7071 = 1.67$ N. The standard 5 N has a 3× margin.

WE-L.E.b (completion). The cube is damp and $\mu$ drops silently to 0.35. Recompute $a_{max}$ at 5 N and the required force at $a = 2$ m/s².

Completion. $a_{max} = 2 \cdot 0.35 \cdot 5 \cdot 0.7071/0.1 - 9.81 = 24.75 - 9.81 = 14.9$ m/s² (1.5 g). Required force at $a=2$: $F \ge 0.1 \cdot 11.81/(2 \cdot 0.35 \cdot 0.7071) = 2.39$ N. Margin at 5 N is 2.1×. The silent friction drop is invisible until slip; only the momentum-observer residual or a tactile signal reveals it.

WE-L.E.c (faded). Design the release trajectory from $z = 0.3$ to $z = 0.16$ over 0.5 s with a minimum-jerk profile, and compute the impact velocity if the cube is released at $z = 0.16$ with zero velocity. Faded step: minimum jerk gives $z(t) = z_0 + (z_f - z_0)(10\tau^3 - 15\tau^4 + 6\tau^5)$ with $\tau = t/T$; the final velocity and acceleration are zero. The completion shows that releasing at $z = 0.16$ with zero velocity produces a 1 cm fall, impact velocity $0.44$ m/s, and a rebound of 0.13 m/s — safely contained.

WE-L.E.d (contrastive near-miss). A learner releases the cube at $z = 0.3$ while the arm is still moving laterally at 0.2 m/s. The cube bounces off the bin rim and lands on the table. The bounce height (1.4 cm) is not the problem; the lateral velocity is. Why it fails: the cube’s horizontal momentum carries it into the rim, and the rim’s angled contact deflects it out. The fix: release with zero lateral velocity and centred over the bin, and descend to 1 cm above the bottom before opening the fingers.

Comparative table: the four contact events.

Design principle P33.5. Budget the force margin explicitly at every contact event; a grasp that holds at rest may fail under acceleration, and a placement that looks gentle may bounce.

Consolidation checkpoint 5

(a) Three capabilities. You can compute the acceleration budget from the grip force. You can design a release trajectory with zero terminal velocity. You can integrate contact detection and recovery into the 1.1.4 state machine.

(b) Transfer probe. The divider were moved to $x = 0.45$, intersecting the nominal path. What changes in the plan? Answer: the 2.3.2 planner must route around it; the contact budget adds a proximity event; and the swing analysis becomes critical because clearance falls below the gripper width.

(c) Remediation rule. If you cannot compute $a_{max}$ from Eq. (E.1), re-derive the force balance before the lab.

Failure taxonomy

The table covers the three lab corruptions plus three failures not exercised in the lab: integral windup, sensor saturation, and planner staleness.

The lab

Task. Implement a torque-level impedance controller on the running arm in MuJoCo, execute the pick-and-place with the rotated grasp, detect unintended contact with the momentum observer, and recover. Then train a clipped residual policy on top of the impedance controller in simulation, and evaluate whether it reduces placement error without increasing peak contact force.

Conditions. MuJoCo 3.x with the running arm model, the cube with $\mu = 0.5$, the divider and bin as specified, 1 kHz control, 2 ms physics timestep, implicit integrator. Python 3.10+, PyTorch 2.x. Seed all RNGs. The eye-in-hand camera is rendered at 30 Hz for object tracking, not for control.

Controls. (i) The 1.1.3 computed-torque position controller as the baseline; (ii) impedance control with fixed $K_d = 1000$ N/m and 5 N grip; (iii) impedance control with adaptive grip force from the momentum-observer residual; (iv) the residual policy on top of (iii).

Metrics.

Pre-registered hypotheses.

H1. The baseline position controller fails on at least 50% of trials (slip or force spike), while the impedance controller succeeds on at least 80%, because the position controller’s steady-state error under contact produces uncontrolled force.

H2. Adaptive grip force from the observer residual reduces slip events by at least 50% relative to fixed 5 N, because the residual detects friction changes before slip becomes macroscopic.

H3. The clipped residual policy reduces placement error by at least 20% relative to the impedance controller alone, because the residual learns the release timing that the fixed controller cannot.

H4. The residual policy does not increase peak contact force above 20 N, because the clip bounds the action to $K_d \delta_{max} = 20$ N.

Break-it sequence.

1. Halve the friction silently. Set $\mu = 0.25$ without telling the controller. The adaptive grip force should raise the force before slip. Silent: the images and the joint trajectories look normal until the cube slips.
2. Raise the contact stiffness tenfold. Set $k_c = 10^6$ N/m. The impedance controller should remain stable; the position controller should chatter. Silent: the force signal looks noisy rather than obviously unstable, and the failure appears as a gradual drift.
3. Inject a force-sensor bias. Add 2 N constant bias to the measured wrench. The momentum observer, which uses torque rather than the sensor, should still detect contact; the sensor-based detection should produce a false positive. Silent: the bias is constant and looks like a payload.

Acceptance criteria.

What good looks like. A good artefact has five properties. (1) The force budget is documented per contact event, with the margin at each. (2) The momentum-observer residual is logged at 1 kHz and plotted against the contact events, so detection latency is measurable. (3) The break-it results are a table, not prose, so the silent corruptions are visible. (4) The residual policy’s action is logged and its clip is verified against the force limit. (5) The object pose from the camera is compared against the gripper pose throughout, so a slip is visible as a divergence, not only as a failure.

A minimal runnable core of the impedance controller and observer, seeded and annotated:

# lab_2_3_3.py — impedance control + momentum observer (MuJoCo)
import numpy as np, mujoco
np.random.seed(0)

model = mujoco.MjModel.from_xml_path("running_arm.xml")   # arm + cube + bin + divider
data  = mujoco.MjData(model)
Kq    = np.diag([80.0]*6)                                 # joint stiffness (Nm/rad)
Dq    = np.diag([4.0]*6)                                  # joint damping
Ko    = np.diag([50.0]*6)                                 # observer gain
r     = np.zeros(6)                                       # residual state

def impedance_torque(q, qd, qdot, $q_des$):
    e = $q_des$ - q                                          # joint error
    return Kq @ e - Dq @ qdot + data.qfrc_bias             # bias = C qdot + g

def momentum_observer(tau, q, qdot, dt):
    global r
    p = data.qM @ qdot                                     # generalized momentum
    r = r + Ko @ (p - (tau + r) * dt)                      # Eq. (C.5), first-order
    return r                                               # converges to J^T $F_ext$

for step in range(4000):                                   # 4 s at 1 kHz
    q, qdot = data.qpos[:6].copy(), data.qvel[:6].copy()
    tau = impedance_torque(q, None, qdot, $q_home$)
    data.ctrl[:] = tau
    mujoco.mj_step(model, data)
    res = momentum_observer(tau, q, qdot, model.opt.timestep)
    if np.linalg.norm(res) > 4.0:                          # contact threshold
        contact_flag = True                                # recovery state entered

Line 7 sets the joint stiffness that renders the Cartesian stiffness of Eq. (C.3); line 8 sets damping; line 9 sets the observer gain, which must be fast enough to track contact but slow enough not to amplify noise. Line 14 uses qfrc_bias, MuJoCo’s precomputed $C\dot q + g$. Line 18 is the discrete momentum observer; line 20 is the threshold from the model uncertainty of §L.E.2.

Cheat sheet

Exercises with solutions

E 2.3.3.1. Derive Eq. (A.2) from Eq. (A.1) and state the units of each factor.

S 2.3.3.1. At steady state $\ddot e = \dot e = 0$, so Eq. (A.1) gives $K_p e = -M^{-1} J^\top F_{ext}$, hence $e = -K_p^{-1} M^{-1} J^\top F_{ext}$. Units: $K_p$ is N·m/rad (or N/m in Cartesian form), $M^{-1}$ is kg$^{-1}$, $J^\top$ is m (for a rotational joint) or dimensionless, $F_{ext}$ is N; the product is metres or radians. The lesson: the error is an equilibrium of two stiffnesses, not a transient.

E 2.3.3.2. A 3-finger grasp has $\mu = 0.4$ and contact normals at $120^\circ$ in the plane. Is the grasp force-closed in the plane? Use the friction-cone-of-the-line condition.

S 2.3.3.2. The half-angle is $\arctan 0.4 = 21.8^\circ$. Each pair of contact normals differs by $120^\circ$, so the line joining any two contacts makes an angle of at least $60^\circ$ with a normal, well outside the cone. The grasp is not force-closed in the plane; it relies on the third finger and on friction. The lesson: three fingers do not guarantee planar force closure; the geometry of the contact normals does.

E 2.3.3.3 (transfer). A robot with $K_x = 2000$ N/m contacts a surface of stiffness $5 \times 10^5$ N/m. It must apply 10 N. Compute the required penetration and the ratio $K_x/k_c$. Is impedance control stable here?

S 2.3.3.3. Penetration $d = F/K_{eff} \approx 10/2000 = 5$ mm. Ratio $K_x/k_c = 0.004$, so the robot is far more compliant than the environment and impedance control is in its stable regime. The lesson: the stability criterion is the stiffness ratio, not the absolute force.

E 2.3.3.4. State the stiffness-matching rule and explain the mechanism that makes admittance stable against a stiff environment.

S 2.3.3.4. Use admittance when $k_e \gg K_x$, impedance when $k_e \ll K_x$. Against a stiff environment, the admittance integrates the measured force into a reference motion; the position loop then tracks the reference, and any position error produces a large force that the admittance immediately converts into corrective motion. The loop is a force-to-motion map, which is passive if the admittance is passive. The lesson: the choice is about which variable is the stable input to the loop.

E 2.3.3.5 (diagnostic). A momentum observer on a sensorless arm reports a nonzero residual during free motion. Name two causes and the discriminating test.

S 2.3.3.5. (1) Dynamic-model error, e.g., an unmodelled payload: test by adding the payload mass to the model and checking whether the residual vanishes. (2) Friction in the joints not captured by the model: test by moving at constant velocity in both directions; friction reverses sign with velocity while a payload does not. The lesson: the observer is only as good as the model it subtracts.

E 2.3.3.6. A residual policy is clipped at 2 cm with $K_d = 1000$ N/m. A safety review asks for the peak force bound. Compute it and state what changes if $K_d$ is raised to 4000 N/m.

S 2.3.3.6. Peak force is $K_d \delta_{max} = 1000 \cdot 0.02 = 20$ N. At $K_d = 4000$, the same clip gives 80 N, exceeding the 40 N gripper limit; the clip must fall to 1 cm to keep the 20 N bound. The lesson: the clip is not a safety guarantee by itself; it is a guarantee only together with the stiffness.

E 2.3.3.7 (transfer). The cube’s mass is unknown within $\pm 20$. How does that affect the grip-force budget, and what randomization range would you set?

S 2.3.3.7. The required force scales linearly with mass, so a 20% mass uncertainty requires a 20% increase in the nominal grip force to preserve the same margin, or a randomization range of $m \in [0.08, 0.12]$ kg during training. The lesson: the force budget must include the identification uncertainty, not only the nominal value.

E 2.3.3.8 (diagnostic). The arm chatters against the cube at 300 Hz. The nominal $k_c$ is $10^5$ N/m and the effective mass is 1 kg. What contact stiffness does the chatter imply, and what does that tell you about the contact?

S 2.3.3.8. $\omega_c = 2\pi \cdot 300 = 1885$ rad/s, so $k_c = m\omega_c^2 \approx 3.55 \times 10^6$ N/m. The actual contact is far stiffer than the rubber-pad model; the fingers are touching the cube’s hard plastic corners, not the compliant pads. The lesson: chatter frequency is a contact-stiffness sensor.

Exit test

You are ready for Lesson 2.3.4 when you can, without notes:

1. Derive the steady-state position error under a constant contact wrench and interpret it.
2. Compute the series stiffness of robot and contact and explain which dominates.
3. State the Signorini complementarity and Coulomb friction conditions.
4. Compute the minimum grip force for a grasp misaligned by $\theta$.
5. Explain why the linearized friction cone enables a linear program.
6. Explain why the encoder cannot resolve contact and the force sensor can.
7. Write the torque-level impedance law and compute $K_x$ from $K_q$.
8. Analyse the admittance steady state and identify the $K_d = 0$ limit as force control.
9. Apply the stiffness-matching rule and explain its mechanism.
10. Describe hybrid position/force control and the role of the selection matrix.
11. Write the momentum observer and state its convergence target.
12. Design a residual-RL policy with a force-bounded clip.
13. Set domain-randomization ranges from identification uncertainty.
14. Budget the grasp force across the pick-and-place and compute $a_{max}$.
15. Design a release trajectory with zero terminal velocity and a contact-recovery sequence.

Cross-references. 1.1.3 supplies the computed-torque law restated in §L.A.1. 2.3.1 supplies force closure and the $\varepsilon$ metric restated in §L.B.1. 2.3.2 supplies the clearance-margin path invoked in §L.E.1. 2.2.4 supplies the Q-shield consumed in §L.D.2. 2.1.5 supplies the eye-in-hand pose estimator and kinematic prior restated in §L.E.2. Lesson 2.3.4 (in-hand and dexterous manipulation) extends the friction-cone and residual-learning machinery to multi-finger contact. Phase 3’s 3.1.4 extends the residual-learning pipeline to a pre-trained VLA.

Ready. The next lesson to write is 2.3.4 — In-Hand and Dexterous Manipulation. Say “next” to generate it.