M8-T8 — Damped Least-Squares Inverse Kinematics

Learning objectives

After this lesson, you should be able to:

• State the damped least-squares inverse J_λ⁺ = Jᵀ(JJᵀ + λ²I)⁻¹ and derive it from a regularised cost.
• Show through the SVD that it replaces 1/σ by σ/(σ² + λ²) and so bounds the joint speeds.
• Run an iterative inverse-kinematics loop and explain the trade-off between tracking error and joint speed.
• Choose λ, fixed or adaptive, and recognise unreachable targets.

Introduction and motivation

A robot tracking a moving target stretches out. The exact solver, correct in theory, sends the elbow joint to 12 rad/s for one tick; the drive faults. With damping, the arm slows as it stretches, the hand lags the target slightly along the singular direction, and the motion stays smooth. Every production arm controller and every animation rig that offers “inverse kinematics” uses some version of damped least squares.

The intuition

An exact inverse treats the task as a hard constraint: reach this velocity at any cost. Damping makes it a trade: reach the velocity as well as is reasonable, but pay a price for large joint speeds. In directions where the arm is strong (large σ) the price is negligible and tracking is nearly exact; in directions where it is weak (σ below λ) the price dominates and the solver simply gives up on that component instead of exploding.

Geometric and physical interpretation

Geometric view. In the hand-velocity plane, the exact solution tries to reproduce any requested vector, including components along the squashed axis of the manipulability ellipse. The damped solution shrinks those components towards zero, in proportion to how small σ is compared with λ. Physical view. The arm "slows down" smoothly as it enters the near-singular region and the hand velocity falls behind the command along the weak direction only; the hand keeps its speed in the strong directions.

The mathematics

λ

the damping factor; larger means safer and less accurate (units: those of J).

J_λ⁺

the damped pseudoinverse Jᵀ(JJᵀ + λ²I)⁻¹.

e

task-space error p_target − p(q).

α

step size of the iteration (often 1, or smaller for stability).

Regularised cost and its solution

q̇ = argmin ( ‖J q̇ − ṗ‖² + λ² ‖q̇‖² ) = Jᵀ ( J Jᵀ + λ² I )⁻¹ ṗ

What it means. The first term is the tracking error, the second penalises large joint speeds. The matrix JJᵀ + λ²I is always invertible, even when J is singular.

The SVD view

Jλ⁺ = Σᵢ [ σᵢ / (σᵢ² + λ²) ] vᵢ uᵢᵀ

What it means. Each singular direction is inverted with gain σ/(σ² + λ²) instead of 1/σ. For σ ≫ λ it is almost 1/σ; for σ ≪ λ it is about σ/λ², tending to 0. The largest possible gain is 1/(2λ), reached at σ = λ.

Iterative inverse kinematics

q ← q + α Jλ⁺(q) ( ptarget − fk(q) )

What it means. Evaluate the error, solve for a small joint step with the damped inverse, update q, repeat. For reachable targets it converges quadratically near the solution; for unreachable ones it settles at a stable best approximation instead of diverging.

Damped inverse = Jᵀ(JJᵀ + λ²I)⁻¹: gain σ/(σ² + λ²) instead of 1/σ. Joint speeds stay bounded (max gain 1/(2λ)); accuracy is traded for robustness.

Step-by-step derivation: the damped inverse, its SVD form and the bound

1. Cost. Minimise F(q̇) = ‖J q̇ − ṗ‖² + λ²‖q̇‖². This is ridge regression (Module 5) with matrix J and target ṗ.
2. Normal equations. Setting the gradient to zero: Jᵀ(J q̇ − ṗ) + λ² q̇ = 0, so (JᵀJ + λ²I) q̇ = Jᵀṗ and q̇ = (JᵀJ + λ²I)⁻¹ Jᵀ ṗ.
3. Convenient form. The push-through identity (JᵀJ + λ²I)⁻¹Jᵀ = Jᵀ(JJᵀ + λ²I)⁻¹ (multiply out both sides) gives the form with an m × m matrix, small for arms (2 × 2 or 6 × 6).
4. SVD form. With J = UΣVᵀ: JᵀJ + λ²I = V(ΣᵀΣ + λ²I)Vᵀ, so q̇ = V(ΣᵀΣ + λ²I)⁻¹ΣᵀUᵀṗ = Σᵢ [σᵢ/(σᵢ² + λ²)] vᵢ (uᵢᵀṗ).
5. Bound. The function g(σ) = σ/(σ² + λ²) has derivative zero at σ = λ with maximum 1/(2λ). Hence ‖q̇‖ ≤ ‖ṗ‖/(2λ) for every Jacobian, however singular: this is the guarantee that plain inversion lacks.
6. Error introduced. The hand velocity actually achieved is J q̇ = Σ [σᵢ²/(σᵢ² + λ²)] uᵢ (uᵢᵀṗ): each component is scaled by σᵢ²/(σᵢ² + λ²) ≤ 1, which is close to 1 for large σ and small for σ ≪ λ. This is the price of damping.
7. Choosing λ. Constant λ is simple but costs accuracy everywhere. An adaptive rule uses damping only near singularities: λ² = λ_max²(1 − (σ_min/ε)²) if σ_min < ε, else 0 (Nakamura and Hanafusa). Typical values: λ ≈ 0.01–0.1 times the largest singular value.
8. Convergence. At the solution the error e is zero so the step is zero: damping changes the path to the solution, not the solution itself, for reachable targets. For an unreachable target the loop converges to a point where Jᵀe balances the damping, a stable best effort.

Relation to other methods. Truncated SVD sets the gain to 0 below a cutoff (a hard switch); damped least squares gives the smooth version. Levenberg-Marquardt in nonlinear least squares is the same idea with the cost replaced by squared residuals (see the SLAM optimisation lessons).

Priority. When tasks conflict (position versus orientation), weighting rows of J or solving in priority order (null-space lesson) decides which component is sacrificed first.

A worked example: velocity damping, then an iterated solve

Arm with l₁ = 1.1, l₂ = 0.8 at q = (30°, 12°), wanted hand velocity ṗ = (0.45, 0.40) (roughly along the arm).

• Singular values. σ = 2.050 and 0.0892, so the condition number is 23.
• Plain inverse. q̇ = J⁻¹ṗ = (2.63, −6.18) rad/s, norm 6.72: too fast for most joints.
• Damped, λ = 0.05. q̇ = (2.01, −4.70), norm 5.11; achieved velocity (0.334, 0.316), tracking error 0.143.
• Damped, λ = 0.1. q̇ = (1.18, −2.73), norm 2.98; achieved (0.181, 0.203), error 0.334.
• Damped, λ = 0.3. norm 0.547, error 0.551: very safe, but the hand barely follows.
• The filter. For the weak direction σ₂ = 0.0892 and λ = 0.1: plain gain 1/σ₂ = 11.2, damped gain 0.0892/(0.00796 + 0.01) = 4.97; the strong direction σ₁ = 2.05 has gain 0.4866 against 0.4877 for the plain inverse, almost unchanged.
• Iterated IK (λ = 0.1). From q = (30°, 60°) to the target (1.2, 1.0), the error falls 0.429 → 0.0644 → 0.00435 → 0.00017 → 0.00001 in four steps, ending at q ≈ (10.96°, 70.41°): fast, quadratic-looking convergence.
• Unreachable target (2.2, 0). The arm can reach only 1.9. With λ = 0.1 the iteration settles at q ≈ (−14.2°, 34.8°) with hand error 0.385 and bounded angles; the plain Newton iteration for the same target runs away to angles of hundreds of degrees.

Robotics application

Manipulators and humanoids. Damped least squares is the default in MoveIt, KDL, Pinocchio and animation IK solvers; it makes teleoperation safe when the operator drags the hand out of reach.

SLAM and registration. Levenberg-Marquardt is damped Gauss-Newton: the same JᵀJ + λI step that stabilises pose-graph optimisation and ICP in degenerate geometry; see nonlinear least squares and robust optimisation.

UAV control and estimation. Damped pseudo-inverses distribute thrust among redundant rotors while limiting saturation and are used in visual servoing of aerial manipulators; Levenberg-Marquardt tunes calibration problems; see extrinsic calibration.

Mobile robots and LiDAR. A degenerate corridor scan gives a nearly singular JᵀJ; adding λI prevents the solver from sliding along the unconstrained direction by an arbitrary amount; see LiDAR point clouds for SLAM.

Engineering implications and limitations

Practical rules

• Use adaptive damping. Zero (or tiny) λ away from singularities for accuracy; ramp it up when σ_min drops below a threshold.
• Express λ in the units of J. It must be comparable to the singular values you want to protect: scale rows (metres vs radians) first.
• Cap the step. Limit the norm of each joint increment and each iteration count; treat non-convergence as “unreachable”.
• Return the error. After solving, report the residual ‖e‖: a large residual means unreachable or damped away, and the caller may need to re-plan.
• Respect joint limits. Clamp, or add weights or null-space terms that push away from limits; damping alone does not respect them.
• Warm-start. Start each solve from the previous solution; it keeps the arm on one elbow branch and halves iterations.

Limits

Damping biases the solution along weak directions: the hand lags the command near singularities. It does not make a singular configuration reachable, nor does it choose between multiple solution branches, nor handle obstacles or joint limits on its own.

Common misconceptions

Engineering takeaways

1. Damped least squares minimises ‖Jq̇ − ṗ‖² + λ²‖q̇‖² and gives q̇ = Jᵀ(JJᵀ + λ²I)⁻¹ṗ.
2. In the SVD, the gain 1/σ becomes σ/(σ² + λ²), bounded by 1/(2λ): joint speeds can never blow up.
3. The achieved velocity is shrunk by σ²/(σ² + λ²): accuracy is traded for robustness, mostly along weak directions.
4. Iterating q ← q + J_λ⁺ e converges to reachable targets and settles at a best effort for unreachable ones.
5. Use adaptive damping, consistent units, step caps and a residual check.

References and further reading

Primary papers and authoritative documentation.

• Buss, Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares: Introduction to inverse kinematics with Jacobian transpose, pseudoinverse and damped least squares methods; the site did not load when we last checked
• Modern Robotics (Lynch and Park), free preprint: chapter 6.2, numerical inverse kinematics
• Boyd and Vandenberghe, Introduction to Applied Linear Algebra (VMLS): regularised least squares and the Levenberg-Marquardt method; the site did not load when we last checked
• UCSD ECE276A: Sensing and Estimation in Robotics: damped least squares and Levenberg-Marquardt in registration

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

A redundant arm has more joints than the task needs. The extra freedom is a null space: joint motions that leave the hand alone. The next lesson shows how to use it for a secondary goal without disturbing the primary one.

Related content

Nothing below is required to finish this lesson.

• Before
  M8-T7: Robot Singularities

  Find singular configurations with the determinant and the SVD and describe what the arm cannot do there.

• Lab
  M8-L1: Interactive Robotic Arm and Inverse Kinematics

  Drive a two-link arm to targets by inverse kinematics, watch the pseudoinverse blow up near a singularity, and fix it with damping.