Making the arm springy

Give way, then come back

Every controller so far fought errors as hard as it could. Near people and fragile parts you want the opposite: a hand that yields when pushed, like a spring, and returns afterwards. Impedance control lets you choose how springy it is.

A spring at the hand

Imagine a spring and a damper tying the grasp point to a rest position $x_\text{des}$:

$x$ is the grasp point's position (m) and $\dot x$ its velocity (m/s); $K$ is the stiffness (N/m), $D$ the damping (N·s/m), and $F$ the force (N) the imaginary spring and damper apply to the hand.

From a hand force to joint torques

The 3×4 Jacobian relates joint velocities to the hand's: $\dot x = J(q)\,\dot q$. A force does the same work measured at the hand or at the joints, $F \cdot \dot x = \tau \cdot \dot q$, so the torques that push the hand with force $F$ are

with $g(q)$ still cancelling gravity. Nothing measures the push: under a steady force $F_\text{ext}$ the hand settles where the spring balances it, $K\,\Delta x = F_\text{ext}$, so it moves $F_\text{ext}/K$, exactly the stiffness you chose.

The wrist is held for you. The three other joints then fix the hand's position; with all four free, the arm could fold in a way that doesn't move the hand at all, and no spring at the hand would stop it.

The program

It holds the hand where it starts for 5.5 s with $K$ = 600 N/m, then for 5.5 s with $K$ = 150 N/m. Each time the hand is pushed 4 N forwards for 1.2 s, then 4 N down, then jabbed. Red arrows show the pushes, and the yellow line is the virtual spring, stretched from the frame at the rest position.

Your task

Write impedance_torque(q, qd, x_des, K, D). Before and after the pushes the hand must rest within 1 mm of its start, the steady pushes must measure a stiffness within 15 % of $K$, and after each jab the hand must be back within 2 mm within 1 s.