Skip to content

Commit 36ff77e

Browse files
chore: add inverse kinematics (#136)
* add inverse kinematics * Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --------- Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
1 parent b33bfe6 commit 36ff77e

8 files changed

Lines changed: 593 additions & 3 deletions

File tree

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
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.

‎doc/kinematics/README.md‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ Geometry and motion algorithms for serial kinematic chains.
44

55
## Algorithms
66

7-
| Algorithm | Description |
8-
|--------------------------------------------|-------------------------------------------------------------------------------------|
9-
| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions from joint angles for serial chains — $O(n)$ complexity |
7+
| Algorithm | Description |
8+
|--------------------------------------------|-------------------------------------------------------------------------------------------------------------|
9+
| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions from joint angles for serial chains — $O(n)$ complexity |
10+
| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles from a target end-effector position via Damped Least Squares — $O(k \cdot n)$ iterative |

‎numerical/kinematics/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,14 +8,17 @@ target_include_directories(numerical.kinematics ${NUMERICAL_VISIBILITY}
88
target_link_libraries(numerical.kinematics ${NUMERICAL_VISIBILITY}
99
numerical.dynamics
1010
numerical.math
11+
numerical.solver
1112
)
1213

1314
target_sources(numerical.kinematics PRIVATE
1415
ForwardKinematics.hpp
16+
InverseKinematics.hpp
1517
)
1618

1719
numerical_add_coverage_sources(numerical.kinematics
1820
ForwardKinematics.cpp
21+
InverseKinematics.cpp
1922
)
2023

2124
add_subdirectory(test)
Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
#include "numerical/kinematics/InverseKinematics.hpp"
2+
3+
namespace kinematics
4+
{
5+
template class InverseKinematics<float, 1>;
6+
template class InverseKinematics<float, 2>;
7+
template class InverseKinematics<float, 3>;
8+
}
Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
#pragma once
2+
3+
#if defined(__GNUC__) || defined(__clang__)
4+
#pragma GCC optimize("O3", "fast-math")
5+
#endif
6+
7+
#include "infra/util/ReallyAssert.hpp"
8+
#include "numerical/dynamics/RevoluteJointLink.hpp"
9+
#include "numerical/kinematics/ForwardKinematics.hpp"
10+
#include "numerical/math/CompilerOptimizations.hpp"
11+
#include "numerical/math/Geometry3D.hpp"
12+
#include "numerical/math/Matrix.hpp"
13+
#include "numerical/solvers/GaussianElimination.hpp"
14+
#include <array>
15+
#include <cmath>
16+
#include <cstddef>
17+
18+
namespace kinematics
19+
{
20+
template<typename T>
21+
struct InverseKinematicsConfig
22+
{
23+
static_assert(std::is_floating_point_v<T>,
24+
"InverseKinematicsConfig only supports floating-point types");
25+
26+
T dampingFactor = T(0.1);
27+
T tolerance = T(1e-4);
28+
std::size_t maxIterations = 100;
29+
};
30+
31+
template<typename T, std::size_t NumLinks>
32+
struct InverseKinematicsResult
33+
{
34+
math::Vector<T, NumLinks> q;
35+
T finalError;
36+
std::size_t iterations;
37+
bool converged;
38+
};
39+
40+
template<typename T, std::size_t NumLinks>
41+
class InverseKinematics
42+
{
43+
static_assert(std::is_floating_point_v<T>,
44+
"InverseKinematics only supports floating-point types");
45+
static_assert(NumLinks > 0, "Number of links must be positive");
46+
47+
public:
48+
using Vector3 = math::Vector3<T>;
49+
using Matrix3 = math::Matrix3<T>;
50+
using JointVector = math::Vector<T, NumLinks>;
51+
using Jacobian = math::Matrix<T, 3, NumLinks>;
52+
using LinkArray = std::array<dynamics::RevoluteJointLink<T>, NumLinks>;
53+
using PositionArray = std::array<Vector3, NumLinks + 1>;
54+
55+
explicit InverseKinematics(InverseKinematicsConfig<T> cfg = {});
56+
57+
OPTIMIZE_FOR_SPEED InverseKinematicsResult<T, NumLinks> Solve(
58+
const LinkArray& links,
59+
const Vector3& target,
60+
const JointVector& initialQ) const;
61+
62+
private:
63+
Jacobian ComputeJacobian(
64+
const LinkArray& links,
65+
const JointVector& q,
66+
const PositionArray& positions) const;
67+
68+
InverseKinematicsConfig<T> config;
69+
ForwardKinematics<T, NumLinks> fk;
70+
mutable solvers::GaussianElimination<T, 3> solver;
71+
};
72+
73+
template<typename T, std::size_t NumLinks>
74+
InverseKinematics<T, NumLinks>::InverseKinematics(InverseKinematicsConfig<T> cfg)
75+
: config(cfg)
76+
{
77+
really_assert(config.dampingFactor > T(0));
78+
really_assert(config.tolerance > T(0));
79+
}
80+
81+
template<typename T, std::size_t NumLinks>
82+
OPTIMIZE_FOR_SPEED
83+
InverseKinematicsResult<T, NumLinks>
84+
InverseKinematics<T, NumLinks>::Solve(
85+
const LinkArray& links,
86+
const Vector3& target,
87+
const JointVector& initialQ) const
88+
{
89+
JointVector q = initialQ;
90+
91+
std::size_t iter = 0;
92+
T error = T(0);
93+
94+
for (; iter < config.maxIterations; ++iter)
95+
{
96+
auto positions = fk.Compute(links, q);
97+
const Vector3& eePos = positions[NumLinks];
98+
99+
Vector3 e = target - eePos;
100+
error = math::VectorNorm(e);
101+
102+
if (error < config.tolerance)
103+
return { q, error, iter, true };
104+
105+
Jacobian J = ComputeJacobian(links, q, positions);
106+
107+
math::SquareMatrix<T, 3> JJt{};
108+
for (std::size_t r = 0; r < 3; ++r)
109+
for (std::size_t c = 0; c < 3; ++c)
110+
{
111+
T sum = T(0);
112+
for (std::size_t k = 0; k < NumLinks; ++k)
113+
sum += J.at(r, k) * J.at(c, k);
114+
JJt.at(r, c) = sum;
115+
}
116+
117+
T lambda2 = config.dampingFactor * config.dampingFactor;
118+
JJt.at(0, 0) += lambda2;
119+
JJt.at(1, 1) += lambda2;
120+
JJt.at(2, 2) += lambda2;
121+
122+
Vector3 y = solver.Solve(JJt, e);
123+
124+
for (std::size_t i = 0; i < NumLinks; ++i)
125+
{
126+
T dq = T(0);
127+
for (std::size_t r = 0; r < 3; ++r)
128+
dq += J.at(r, i) * y.at(r, 0);
129+
q.at(i, 0) += dq;
130+
}
131+
}
132+
133+
auto positions = fk.Compute(links, q);
134+
Vector3 e = target - positions[NumLinks];
135+
error = math::VectorNorm(e);
136+
137+
return { q, error, iter, false };
138+
}
139+
140+
template<typename T, std::size_t NumLinks>
141+
typename InverseKinematics<T, NumLinks>::Jacobian
142+
InverseKinematics<T, NumLinks>::ComputeJacobian(
143+
const LinkArray& links,
144+
const JointVector& q,
145+
const PositionArray& positions) const
146+
{
147+
const Vector3& eePos = positions[NumLinks];
148+
149+
Matrix3 R = Matrix3::Identity();
150+
Jacobian J{};
151+
152+
for (std::size_t i = 0; i < NumLinks; ++i)
153+
{
154+
Vector3 worldAxis = R * links[i].jointAxis;
155+
Vector3 jointToEe = eePos - positions[i];
156+
Vector3 col = math::CrossProduct(worldAxis, jointToEe);
157+
158+
J.at(0, i) = col.at(0, 0);
159+
J.at(1, i) = col.at(1, 0);
160+
J.at(2, i) = col.at(2, 0);
161+
162+
R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0));
163+
}
164+
165+
return J;
166+
}
167+
168+
#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
169+
extern template class InverseKinematics<float, 1>;
170+
extern template class InverseKinematics<float, 2>;
171+
extern template class InverseKinematics<float, 3>;
172+
#endif
173+
}

‎numerical/kinematics/test/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,4 +9,5 @@ target_link_libraries(numerical.kinematics_test PUBLIC
99

1010
target_sources(numerical.kinematics_test PRIVATE
1111
TestForwardKinematics.cpp
12+
TestInverseKinematics.cpp
1213
)

0 commit comments

Comments
 (0)