drag to orbit · scroll to zoom   t = 0.00 s
currenttarget

Controller

τ = Kp·(qd−q) − Kd·q̇
80
6.0
per-joint multipliers

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.

Spring and damper
400N/m
20N·m/rad
1.00
Push into the wall
15N
0.50N/N
101/s
20000N/m

Target pose

Physics

Step response — —

—
overshoot
—
settling (2%)
—
steady err

EE forces, x axis (N)

— external push (on EE) — actual contact (robot on wall) — sensor reading (what the force loop sees) — controller output Fx - - Fd setpoint
—
peak contact
—
contact error
—
settling (2%)