|
| 1 | +# Inverse Kinematics (Damped Least Squares) |
| 2 | + |
| 3 | +## Overview & Motivation |
| 4 | + |
| 5 | +Inverse Kinematics (IK) solves the problem dual to Forward Kinematics: given a desired 3D end-effector position $p_{\text{target}}$, find joint angles $q$ such that the end-effector reaches that position. This is the core computational problem in robot arm control, motion planning, and teleoperation. |
| 6 | + |
| 7 | +The **Damped Least Squares** (DLS) method, also known as the Levenberg-Marquardt approach, iteratively refines a joint-angle estimate by solving a small linear system at each step. Unlike the plain pseudoinverse, DLS adds a damping term $\lambda^2 I$ that keeps the system numerically stable even when the manipulator is near a singular configuration (such as a fully extended arm). |
| 8 | + |
| 9 | +## Mathematical Theory |
| 10 | + |
| 11 | +### Jacobian of a Serial Chain |
| 12 | + |
| 13 | +For a serial revolute-joint chain with $n$ joints, the **geometric Jacobian** $J \in \mathbb{R}^{3 \times n}$ maps joint velocities $\dot{q}$ to end-effector linear velocity: |
| 14 | + |
| 15 | +$$\dot{p}_{\text{ee}} = J(q) \, \dot{q}$$ |
| 16 | + |
| 17 | +Column $i$ of $J$ is the linear velocity contribution of joint $i$: |
| 18 | + |
| 19 | +$$J_i = a_i^w \times (p_{\text{ee}} - p_i)$$ |
| 20 | + |
| 21 | +where: |
| 22 | +- $a_i^w = R_{0 \to i} \, a_i^{\text{link}}$ is the joint axis in world frame (accumulated rotation applied to the link-frame axis) |
| 23 | +- $p_i$ is the world-frame position of joint $i$ (from Forward Kinematics) |
| 24 | +- $p_{\text{ee}}$ is the current end-effector position |
| 25 | + |
| 26 | +### DLS Update Rule |
| 27 | + |
| 28 | +Define the position error: |
| 29 | + |
| 30 | +$$e = p_{\text{target}} - p_{\text{ee}}(q) \in \mathbb{R}^3$$ |
| 31 | + |
| 32 | +The DLS joint update per iteration is: |
| 33 | + |
| 34 | +$$\Delta q = J^\top \bigl(J J^\top + \lambda^2 I\bigr)^{-1} e$$ |
| 35 | + |
| 36 | +This is equivalent to solving the $3 \times 3$ linear system: |
| 37 | + |
| 38 | +$$\underbrace{\bigl(J J^\top + \lambda^2 I\bigr)}_{A} \, y = e, \quad \Delta q = J^\top y$$ |
| 39 | + |
| 40 | +where $A \in \mathbb{R}^{3 \times 3}$ is always symmetric positive definite (the damping $\lambda^2 I$ guarantees this), so `GaussianElimination<T, 3>` solves it reliably. |
| 41 | + |
| 42 | +The updated joint angles are: |
| 43 | + |
| 44 | +$$q_{k+1} = q_k + \Delta q$$ |
| 45 | + |
| 46 | +### Convergence |
| 47 | + |
| 48 | +The loop terminates when: |
| 49 | +- $\|e\| < \epsilon_{\text{tol}}$ (success), or |
| 50 | +- The maximum iteration count is reached (failure to converge — target unreachable or tolerance too tight) |
| 51 | + |
| 52 | +### Why DLS over Plain Pseudoinverse |
| 53 | + |
| 54 | +At a kinematic singularity (e.g., fully extended arm), $J J^\top$ becomes rank-deficient. The pseudoinverse $J^+ = J^\top (J J^\top)^{-1}$ would produce infinite joint velocities. The damping term $\lambda^2 I$ bounds the solution: |
| 55 | + |
| 56 | +$$\|\Delta q\| \leq \frac{\|e\|}{\lambda}$$ |
| 57 | + |
| 58 | +The cost is a small positional bias proportional to $\lambda$: the algorithm converges to within approximately $\lambda^2 \|e\|$ rather than exact zero. Choosing $\lambda \in [0.01, 0.2]$ balances stability and accuracy for typical robot geometries. |
| 59 | + |
| 60 | +## Complexity Analysis |
| 61 | + |
| 62 | +| Case | Time | Space | Notes | |
| 63 | +|---------------|----------------|--------|----------------------------------------| |
| 64 | +| Per iteration | $O(n)$ | $O(n)$ | FK sweep + Jacobian + $3\times3$ solve | |
| 65 | +| Total | $O(k \cdot n)$ | $O(n)$ | $k$ = iterations (typically 10–50) | |
| 66 | + |
| 67 | +The $3 \times 3$ linear solve is $O(1)$ (constant-size), independent of the number of joints $n$. |
| 68 | + |
| 69 | +## Step-by-Step Walkthrough |
| 70 | + |
| 71 | +Consider a 2-link planar arm with $l_1 = 1.0$, $l_2 = 0.8$, both joints rotating about the Z-axis, target $p_{\text{target}} = (0.5, 1.0, 0)$, initial guess $q = (0, 0)$, $\lambda = 0.1$. |
| 72 | + |
| 73 | +**Iteration 1:** |
| 74 | + |
| 75 | +1. FK gives $p_{\text{ee}} = (1.8, 0, 0)$ (fully extended along X) |
| 76 | +2. Error: $e = (-1.3, 1.0, 0)$, $\|e\| \approx 1.64$ |
| 77 | +3. Jacobian (Z-axis joints, cross-product form): |
| 78 | + - $J_1 = [0,0,1]^\top \times (p_{\text{ee}} - p_0) = [0,0,1]^\top \times [1.8, 0, 0]^\top = [0, 1.8, 0]^\top$ |
| 79 | + - $J_2 = [0,0,1]^\top \times (p_{\text{ee}} - p_1) = [0,0,1]^\top \times [0.8, 0, 0]^\top = [0, 0.8, 0]^\top$ |
| 80 | + - $J = \begin{bmatrix} 0 & 0 \\ 1.8 & 0.8 \\ 0 & 0 \end{bmatrix}$ (only Y-row is non-zero for planar X motion) |
| 81 | +4. $A = J J^\top + 0.01 I$ — a $3 \times 3$ SPD matrix |
| 82 | +5. Solve $Ay = e$, compute $\Delta q = J^\top y$ |
| 83 | +6. Update both joint angles toward a bent configuration |
| 84 | + |
| 85 | +After ~40 iterations $q$ converges to joints that produce $p_{\text{ee}} \approx (0.5, 1.0, 0)$. |
| 86 | + |
| 87 | +## Pitfalls & Edge Cases |
| 88 | + |
| 89 | +- **Unreachable targets**: If $\|p_{\text{target}}\|$ exceeds the total arm length, the algorithm reaches maximum iterations without converging. The returned result has `converged = false`; the end-effector will be as close as geometrically possible. |
| 90 | +- **Singularity handling**: Near singularities (e.g., fully extended arm), the damping $\lambda$ prevents numerical blow-up but introduces a small steady-state error. Reduce $\lambda$ for higher accuracy away from singularities. |
| 91 | +- **Local minima in redundant chains**: For $n > 3$ (redundant), DLS finds *a* solution (minimum-norm joint motion) but not necessarily the one with the best posture. Null-space techniques (projecting secondary objectives into $\ker(J)$) can improve this. |
| 92 | +- **Initial guess sensitivity**: Starting closer to the expected solution reduces iterations and avoids winding through joint-limit regions. For repeated calls (e.g., tracking), use the previous solution as the initial guess. |
| 93 | +- **Float-only**: Uses `std::cos`, `std::sin`, and `std::sqrt`; Q15/Q31 fixed-point types are not supported. |
| 94 | +- **Configuration preconditions**: `dampingFactor` and `tolerance` must both be strictly positive. The damping term $\lambda^2 I$ ensures $A$ is symmetric positive definite only when $\lambda > 0$; both values are asserted in debug builds via `really_assert`. |
| 95 | + |
| 96 | +## Variants & Generalizations |
| 97 | + |
| 98 | +- **Jacobian Transpose**: $\Delta q = \alpha J^\top e$ — no matrix solve, but slower convergence and step-size sensitive. Suitable for very constrained embedded targets. |
| 99 | +- **Pseudoinverse (Moore-Penrose)**: DLS with $\lambda = 0$ — exact minimum-norm solution but numerically unstable at singularities. |
| 100 | +- **FABRIK**: Forward-And-Backward Reaching IK — purely geometric, position-only, no Jacobian required. Very fast per iteration; preferred when orientation control is not needed. |
| 101 | +- **Cyclic Coordinate Descent (CCD)**: Each joint independently minimizes error; simple but produces unnatural motion. |
| 102 | +- **Full 6-DOF pose IK**: Extend $J$ to $6 \times n$ (stacking angular velocity rows) and $e$ to $\mathbb{R}^6$ to control both position and orientation. |
| 103 | + |
| 104 | +## Applications |
| 105 | + |
| 106 | +- **Robot arm target tracking**: Drive the end-effector to follow a moving 3D target in real-time. |
| 107 | +- **Teleoperation**: Map operator hand position to robot joint angles continuously. |
| 108 | +- **Motion planning**: Compute joint-space waypoints from Cartesian-space paths. |
| 109 | +- **Animation & simulation**: Pose limbs in a physically plausible configuration to reach a point of interest. |
| 110 | + |
| 111 | +## Connections to Other Algorithms |
| 112 | + |
| 113 | +- [Forward Kinematics](ForwardKinematics.md): Called internally every iteration to compute current end-effector position and joint positions for the Jacobian. |
| 114 | +- [GaussianElimination](../solvers/GaussianElimination.md): Solves the $3 \times 3$ DLS system $Ay = e$ at the core of each iteration. |
| 115 | +- `numerical/math/Geometry3D.hpp`: Provides `CrossProduct` for Jacobian column computation, `RotationAboutAxis` for accumulated rotation, and `VectorNorm` for convergence check. |
| 116 | + |
| 117 | +## References & Further Reading |
| 118 | + |
| 119 | +- Buss, S.R. (2004). *Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods*. University of California, San Diego. [PDF](https://mathweb.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf) |
| 120 | +- Nakamura, Y. and Hanafusa, H. (1986). "Inverse kinematic solutions with singularity robustness for robot manipulator control". *Journal of Dynamic Systems, Measurement, and Control*, 108(3). |
| 121 | +- Siciliano, B. et al. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 3. |
| 122 | +- Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Chapter 4. |
0 commit comments