Δq ∈ [-0.05 … +0.05] rad for each arm joint (left arm by default). r = −5·â€–Δp‖ – 0.1·â€–Δq‖ + 1 when ‖Δp‖<2 cm −1 on self-collision –0.5 on limit hit.