The hand acts like a spring and damper pulled toward the target. A weaker spring on the joints holds the elbow near the target posture without moving the hand.
τ = Jᵀ( K·e − D·ẋ + Fd ) + N·τnull
τ = Jᵀ·F + N·τnull
F = S·Ff + (I − S)(K·e − D·ẋ)force loop along x, impedance in the other 5 directions
S = diag(1, 0, 0, 0, 0, 0)selects x, the direction into the wall
Ff = Fd + Kf·eF + KI∫eF − Dx·ẋforce loop on the measured contact, eF = Fd − Fmeas; damping is lighter once touching
K = diag(Kx·I₃, Kr·I₃)spring: stiffness on position, then on orientation
D = 2ζ√Kdamper, sized by ζ as if the hand weighed 1 kg
e = (pd − p, θerr)position error in m, rotation error in rad (axis-angle)
τnull = Kn(qd − q) − 2√Kn·q̇posture spring that holds the elbow, fixed at Kn = 30 N·m/rad; N removes any part that would move the hand
Fd along xconstant push, open loop: no force sensor in this law
Highlighted symbols are the sliders below.