A Field Guide to Embedded Robot Manipulator Kinematics, Dynamics, and Control
October 24, 2026
Forward Kinematics computes the 3D Cartesian positions of all joints of a serial kinematic chain, plus the tool (end-effector) point, given the joint angles. For a chain of \(n\) revolute joints it produces \(n + 1\) position vectors — the origin of every joint followed by the tool point — in \(O(n)\) time.
This is the geometric foundation for visualization, collision detection, workspace analysis and Jacobian-based inverse kinematics. It answers the question: “given these joint angles, where is each joint, and where is the tool, in the base frame?”
Each link \(i\) carries:
The tool point is described separately by a constant offset \(t\) expressed in the frame of the last link. The tool point is a purely kinematic parameter: it is unrelated to where the last link’s center of mass happens to be.
The orientation of link \(i\) relative to the base is the ordered product
\[^{0}R_{i} = \prod_{k=0}^{i} R_k(q_k) = R_0(q_0)\,R_1(q_1)\cdots R_i(q_i)\]
where each factor is the rotation by angle \(q_k\) about \(\hat{z}_k\), obtained from Rodrigues’ formula
\[R(\hat{u}, \theta) = \cos\theta\,\mathbf{I}_3 + (1 - \cos\theta)\,\hat{u}\hat{u}^T + \sin\theta\,[\hat{u}]_\times\]
The product is taken left to right because every axis is expressed in its own link frame: \(^{0}R_{i}\) maps vectors written in link \(i\) coordinates into base coordinates. Rotations are right-handed: a positive angle about \(\hat{y}\) carries \(\hat{x}\) towards \(-\hat{z}\).
\[p_0 = r_0, \qquad p_{i+1} = p_i + {}^{0}R_{i}\, r_{i+1} \quad (0 \le i < n - 1), \qquad p_{\text{tool}} = p_{n-1} + {}^{0}R_{n-1}\, t\]
where \(p_i\) is the base-frame position of joint \(i\). The tool point is stored as the last element of the result.
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(n)\) | \(O(n)\) | One Rodrigues rotation, one 3×3 product and one 3×3 matrix–vector product per joint |
Consider a 2-link arm with \(y\)-axis joints, joint 2 located \(1\) along \(\hat{x}\) of link 1, a tool offset \(t = [0.8, 0, 0]^T\), no base offset, and \(q = [\pi/4, -\pi/6]\).
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.
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).
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:
\[\dot{p}_{\text{ee}} = J(q) \, \dot{q}\]
Column \(i\) of \(J\) is the linear velocity contribution of joint \(i\):
\[J_i = a_i^w \times (p_{\text{ee}} - p_i)\]
where: - \(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) - \(p_i\) is the world-frame position of joint \(i\) (from Forward Kinematics) - \(p_{\text{ee}}\) is the current tool (end-effector) point, i.e. the last joint position plus the rotated tool offset
Define the position error:
\[e = p_{\text{target}} - p_{\text{ee}}(q) \in \mathbb{R}^3\]
The DLS joint update per iteration is:
\[\Delta q = J^\top \bigl(J J^\top + \lambda^2 I\bigr)^{-1} e\]
This is equivalent to solving the \(3 \times 3\) linear system:
\[\underbrace{\bigl(J J^\top + \lambda^2 I\bigr)}_{A} \, y = e, \quad \Delta q = J^\top y\]
where \(A \in \mathbb{R}^{3 \times 3}\) is always symmetric positive definite (the damping \(\lambda^2 I\) guarantees this), so a small Gaussian elimination (or Cholesky factorization) solves it reliably.
The updated joint angles are:
\[q_{k+1} = q_k + \Delta q\]
The loop terminates when: - \(\|e\| < \epsilon_{\text{tol}}\) (success), or - The maximum iteration count is reached (failure to converge — target unreachable or tolerance too tight)
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:
\[\|\Delta q\| \leq \frac{\|e\|}{2\lambda}\]
because each singular value \(\sigma\) of \(J\) is mapped to \(\sigma / (\sigma^2 + \lambda^2) \le 1/(2\lambda)\).
Damping does not bias the converged solution of the iteration. A fixed point requires \(J^\top (J J^\top + \lambda^2 I)^{-1} e = 0\), which for a full-rank \(J\) implies \(e = 0\): a reachable, non-singular target is reached exactly, and larger \(\lambda\) only shortens the steps and slows convergence. The damping trade-off is therefore speed and step size versus robustness near singularities; only at a singular configuration, or for an unreachable target, does the iteration stop at a least-squares stationary point with \(e \neq 0\). Choosing \(\lambda \in [0.01, 0.2]\) (scaled to the robot’s link lengths) is a common starting point.
| Case | Time | Space | Notes |
|---|---|---|---|
| Per iteration | \(O(n)\) | \(O(n)\) | FK sweep + Jacobian + \(3\times3\) solve |
| Total | \(O(k \cdot n)\) | \(O(n)\) | \(k\) = iterations (typically 10–50) |
The \(3 \times 3\) linear solve is \(O(1)\) (constant-size), independent of the number of joints \(n\).
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\).
Iteration 1:
After 7 iterations the error drops below \(10^{-4}\) and \(p_{\text{ee}} \approx (0.5, 1.0, 0)\); with \(\lambda = 0.5\) the same target still converges exactly, in 13 iterations.
The Articulated Body Algorithm (ABA) is the most efficient method for computing forward dynamics of serial kinematic chains. Given joint positions \(q\), velocities \(\dot{q}\), and applied torques \(\tau\), it computes joint accelerations \(\ddot{q}\) in \(O(n)\) time — linear in the number of links.
This is the forward-dynamics counterpart of the Recursive Newton-Euler Algorithm (which solves inverse dynamics). Together, they form the two fundamental \(O(n)\) algorithms in rigid-body dynamics.
The mass-matrix alternative forms \(M(q)\) (for example with the Composite Rigid Body Algorithm, \(O(n^2)\)) and factorizes it (\(O(n^3)\)). For short chains the two approaches cost about the same; in Featherstone’s operation counts ABA becomes the cheaper option somewhere around \(n \approx 6\)–\(9\) bodies, and its advantage grows linearly beyond that.
Given a serial chain of \(n\) rigid bodies connected by revolute joints, find:
\[\ddot{q} = M(q)^{-1} [\tau - C(q, \dot{q})\dot{q} - g(q)]\]
ABA computes this without ever forming or inverting \(M\).
ABA has three passes over the kinematic chain:
Forward pass (base → tip): Compute link kinematics — angular velocities, linear velocities, and velocity-product accelerations from joint positions and velocities.
Backward pass (tip → base): Compute articulated-body inertias \(I^A_i\) and bias wrenches \(p^A_i\) for each link. At each step, the joint projection removes one degree of freedom from the spatial inertia, and the remainder is propagated to the parent.
Forward pass (base → tip): Resolve joint accelerations using the articulated-body quantities. Each joint acceleration \(\ddot{q}_i\) is computed from the projected articulated inertia and the transformed parent acceleration.
Articulated-body inertia starts as the rigid-body spatial inertia of each link, then accumulates child contributions:
\[I^A_i \leftarrow I^A_i + X^*_{i+1} \hat{I}^A_{i+1} X_{i+1}\]
where \(\hat{I}^A\) is the inertia after removing the joint DOF (the “downdate”):
\[\hat{I}^A_i = I^A_i - \frac{U_i U_i^T}{D_i}\]
with \(U_i = I^A_i s_i\), \(D_i = s_i^T U_i\), and \(s_i\) the joint motion subspace (joint axis for revolute joints).
Joint acceleration:
\[\ddot{q}_i = \frac{u_i - U_i^T \hat{a}_i}{D_i}\]
where \(u_i = \tau_i - s_i^T p^A_i\) and \(\hat{a}_i = {}^{i}X_{\lambda(i)}\, a_{\lambda(i)} + c_i\) is the parent’s acceleration expressed in link \(i\) plus the velocity-product acceleration \(c_i = v_i \times s_i \dot{q}_i\). The link’s own acceleration is then \(a_i = \hat{a}_i + s_i \ddot{q}_i\).
Bias-force propagation: the articulated bias force passed to the parent is
\[p^a_i = p^A_i + \hat{I}^A_i c_i + U_i \frac{u_i}{D_i}, \qquad p^A_{\lambda(i)} \leftarrow p^A_{\lambda(i)} + {}^{\lambda(i)}X^*_i\, p^a_i\]
where \(p^A_i\) starts as the rigid-body bias force \(v_i \times^* I_i v_i\) (gyroscopic and centripetal terms).
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(n)\) | \(O(n)\) | Three linear passes over the chain |
Each pass visits every link exactly once, performing constant-time spatial algebra (3×3 matrix operations) per link.
Consider a 2-link arm of uniform rods with unit masses and unit lengths, both lying along \(\hat{x}\) at \(q = 0\), rotating about \(\hat{y}\), with zero velocities and zero applied torques under gravity \(g = [0, 0, -9.81]^T\).
Pass 1 — Forward kinematics: both links at \(q = [0, 0]^T\) with \(\dot{q} = [0, 0]^T\), so all velocities and velocity-product terms are zero.
Pass 2 — Backward pass: starting from link 2, compute the rigid-body inertia, project out the joint DOF, transform to link 1’s frame, and accumulate. The articulated inertia at the base now represents the effective inertia of the entire chain.
Pass 3 — Forward pass: at the base, the gravitational acceleration is transformed into the base frame and used to compute \(\ddot{q}_1\). The resulting acceleration is propagated to link 2 to compute \(\ddot{q}_2\).
The result, for uniform rods (\(I_{\text{CoM}} = ml^2/12\)): \(\ddot{q} = g\,[\,9/7,\ -12/7\,]^T \approx [12.61, -16.82]^T\). The shoulder accelerates downward, but the elbow accelerates the other way: the outer link initially rotates upward relative to the inner one (its absolute angular acceleration is \(\ddot{q}_1 + \ddot{q}_2 = -3g/7\)), the classic whip-like start of a double pendulum released from rest.
float (not
fixed-point) for dynamics computations.RNEA(q, qDot, ABA(q, qDot, tau)) ≈ tau.The Euler-Lagrange equations provide a systematic way to derive the equations of motion for mechanical systems from energy principles. Instead of tracking forces on each body (as in Newton’s method), they express dynamics in terms of generalized coordinates \(q\) and the system’s kinetic and potential energy.
For a robotic manipulator or any articulated mechanism with \(n\) degrees of freedom, the Euler-Lagrange formulation yields a compact matrix equation:
\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau\]
Where: - \(q \in \mathbb{R}^n\) — generalized coordinates (joint angles/positions) - \(\dot{q}, \ddot{q}\) — generalized velocities and accelerations - \(M(q) \in \mathbb{R}^{n \times n}\) — mass (inertia) matrix, symmetric positive definite - \(C(q, \dot{q})\dot{q} \in \mathbb{R}^n\) — Coriolis and centrifugal terms - \(g(q) \in \mathbb{R}^n\) — gravitational terms - \(\tau \in \mathbb{R}^n\) — generalized forces/torques (inputs)
This formulation supports two fundamental computations: - Forward dynamics: given torques \(\tau\), compute accelerations \(\ddot{q} = M^{-1}(\tau - C\dot{q} - g)\) - Inverse dynamics: given desired accelerations \(\ddot{q}\), compute required torques \(\tau = M\ddot{q} + C\dot{q} + g\)
The Lagrangian of a system is defined as:
\[\mathcal{L}(q, \dot{q}) = T(q, \dot{q}) - V(q)\]
where \(T\) is kinetic energy and \(V\) is potential energy.
The Euler-Lagrange equation for coordinate \(q_i\) is:
\[\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}_i} - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i \quad \text{for } i = 1, \ldots, n\]
For a rigid-body system, kinetic energy takes the form:
\[T = \frac{1}{2}\dot{q}^T M(q) \dot{q}\]
Applying the Euler-Lagrange equation to this quadratic form yields the standard manipulator equation:
\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau\]
The mass matrix is always symmetric (\(M = M^T\)) and positive definite (\(\dot{q}^T M \dot{q} > 0\) for \(\dot{q} \neq 0\)). These properties guarantee: - The forward dynamics linear system \(M\ddot{q} = f\) always has a unique solution - Physical kinetic energy is always non-negative
The Coriolis matrix captures velocity-dependent forces using Christoffel symbols of the first kind:
\[c_{ij} = \sum_{k=1}^{n} \frac{1}{2}\left(\frac{\partial m_{ij}}{\partial q_k} + \frac{\partial m_{ik}}{\partial q_j} - \frac{\partial m_{jk}}{\partial q_i}\right)\dot{q}_k\]
A key property: \(\dot{M} - 2C\) is skew-symmetric, which is important for passivity-based control.
\[g_i(q) = \frac{\partial V}{\partial q_i}\]
where \(V(q)\) is the total potential energy of the system.
| Operation | Time | Space | Notes |
|---|---|---|---|
| Forward dynamics | \(O(n^3)\) | \(O(n^2)\) | Dominated by \(M^{-1}\) solve via Gaussian elimination |
| Inverse dynamics | \(O(n^2)\) | \(O(n^2)\) | Matrix-vector multiplication \(M \ddot{q}\) |
| Model evaluation | Model-dependent | \(O(n^2)\) | Computing \(M(q)\), \(C(q,\dot{q})\), \(g(q)\) |
For systems with \(n \leq 6\) DOF (typical robotic manipulators), the \(O(n^3)\) cost of Gaussian elimination is negligible. For larger systems, consider the Recursive Newton-Euler Algorithm (RNEA) which achieves \(O(n)\) inverse dynamics.
A point mass \(m\) on a rigid rod of length \(l\):
System parameters: \(m = 1\,\text{kg}\), \(l = 1\,\text{m}\), \(g_0 = 9.81\,\text{m/s}^2\)
Model matrices: - \(M(q) = ml^2 = 1.0\) - \(C(q, \dot{q})\dot{q} = 0\) (single DOF → no Coriolis) - \(g(q) = mgl\sin(q) = 9.81\sin(q)\)
Forward dynamics at \(q = 0.5\,\text{rad}\), \(\dot{q} = 0\), \(\tau = 0\):
\[\ddot{q} = M^{-1}(\tau - C\dot{q} - g) = \frac{0 - 0 - 9.81\sin(0.5)}{1.0} = -4.703\,\text{rad/s}^2\]
Inverse dynamics for holding position (\(\ddot{q} = 0\)):
\[\tau = M\ddot{q} + C\dot{q} + g = 0 + 0 + 9.81\sin(0.5) = 4.703\,\text{N·m}\]
For a 2-DOF arm with equal links (\(m_1 = m_2 = 1\,\text{kg}\), \(l_1 = l_2 = 1\,\text{m}\)):
At rest (\(q = [0, 0]^T\), \(\dot{q} = [0, 0]^T\)): - \(\sin(q_1) = 0\), \(\sin(q_1 + q_2) = 0\) - \(g = [0, 0]^T\) → gravity terms vanish when links hang vertically - Forward dynamics with \(\tau = 0\) produces \(\ddot{q} = [0, 0]^T\) (equilibrium)
float unless
inputs are carefully scaled.| Variant | Trade-off | Use Case |
|---|---|---|
| Euler-Lagrange (this) | \(O(n^3)\) forward, closed-form, intuitive | Small systems (\(n \leq 6\)), control design, analysis |
| Newton-Euler | Body-by-body force balance | Single rigid bodies, intuitive for simple systems |
| Recursive Newton-Euler (RNEA) | \(O(n)\) inverse dynamics | Large kinematic chains, real-time control |
| Articulated Body Algorithm (ABA) | \(O(n)\) forward dynamics | Large chains, simulation |
| Hamiltonian formulation | Phase-space, symplectic integration | Long-horizon simulation, energy preservation |
The Newton-Euler formulation describes the dynamics of a single rigid body by directly applying Newton’s second law for translational motion and Euler’s equation for rotational motion. Unlike the Euler-Lagrange approach which works in generalized (joint-space) coordinates, Newton-Euler operates in Cartesian space, computing forces and torques on individual bodies.
In the body-fixed reference frame, the equations are:
\[F = m(\dot{v} + \omega \times v)\] \[\tau = I\dot{\omega} + \omega \times (I\omega)\]
Where: - \(F \in \mathbb{R}^3\) — applied force in the body frame - \(\tau \in \mathbb{R}^3\) — applied torque in the body frame - \(m\) — body mass (scalar) - \(I \in \mathbb{R}^{3 \times 3}\) — inertia tensor in the body frame - \(v, \dot{v} \in \mathbb{R}^3\) — linear velocity and acceleration in the body frame - \(\omega, \dot{\omega} \in \mathbb{R}^3\) — angular velocity and acceleration in the body frame - \(\omega \times v\) — Coriolis term due to body-frame formulation - \(\omega \times (I\omega)\) — gyroscopic coupling term
This supports two computations: - Forward dynamics: given forces/torques, compute linear and angular accelerations - Inverse dynamics: given desired accelerations, compute required forces/torques
In an inertial frame, Newton’s second law is simply \(F = ma\). In a body-fixed (rotating) frame, the transport theorem introduces an additional term:
\[F = m(\dot{v} + \omega \times v)\]
The \(\omega \times v\) term is the Coriolis acceleration that arises because the reference frame itself is rotating.
Forward: \(\dot{v} = F/m - \omega \times v\)
Inverse: \(F = m(\dot{v} + \omega \times v)\)
Euler’s equation governs rotational dynamics:
\[\tau = I\dot{\omega} + \omega \times (I\omega)\]
The \(\omega \times (I\omega)\) term is the gyroscopic torque — it couples different rotational axes when the inertia tensor is non-spherical. For a body spinning about a non-principal axis, this term causes precession even without external torque.
Forward: \(\dot{\omega} = I^{-1}(\tau - \omega \times (I\omega))\)
This requires solving the \(3 \times 3\) linear system \(I\dot{\omega} = \tau - \omega \times (I\omega)\) via Gaussian elimination.
Inverse: \(\tau = I\dot{\omega} + \omega \times (I\omega)\)
This is a direct matrix-vector multiply plus cross product — no linear solve needed.
The inertia tensor \(I\) is: - Symmetric: \(I = I^T\) (6 independent components, not 9) - Positive definite: \(\omega^T I \omega > 0\) for \(\omega \neq 0\) - Constant in body frame: For a rigid body, \(I\) does not change with the body’s orientation when expressed in the body frame
When \(I\) is diagonal (principal axes frame), components are the principal moments of inertia \(I_{xx}, I_{yy}, I_{zz}\), and Euler’s equation simplifies to the well-known form:
\[\tau_x = I_{xx}\dot{\omega}_x + (I_{zz} - I_{yy})\omega_y\omega_z\] \[\tau_y = I_{yy}\dot{\omega}_y + (I_{xx} - I_{zz})\omega_z\omega_x\] \[\tau_z = I_{zz}\dot{\omega}_z + (I_{yy} - I_{xx})\omega_x\omega_y\]
| Operation | Time | Space | Notes |
|---|---|---|---|
| Forward dynamics | \(O(1)\) | \(O(1)\) | Fixed 3×3 system; Gaussian elimination on 3×3 matrix |
| Inverse dynamics | \(O(1)\) | \(O(1)\) | Matrix-vector multiply + cross products |
| Cross product | \(O(1)\) | \(O(1)\) | 6 multiplications and 3 subtractions |
All operations are constant-time since the spatial dimension is fixed at 3. The \(3 \times 3\) Gaussian elimination is effectively a small constant cost.
Parameters: mass \(m = 2\,\text{kg}\), radius \(r = 0.5\,\text{m}\)
Inertia tensor: \(I = \frac{2}{5}mr^2 \cdot \mathbf{I}_3 = 0.2 \cdot \mathbf{I}_3\)
Forward dynamics with \(F = [6, 0, 0]^T\,\text{N}\), \(\tau = [0, 0, 1]^T\,\text{N·m}\), \(v = \omega = 0\):
\[\dot{v} = F/m - 0 = [3, 0, 0]^T \,\text{m/s}^2\] \[\dot{\omega} = I^{-1}\tau = [0, 0, 5]^T \,\text{rad/s}^2\]
Asymmetric body with \(I = \text{diag}(1, 2, 3)\,\text{kg·m}^2\)
Spinning at \(\omega = [1, 1, 0]^T\,\text{rad/s}\) with no external torque:
\[I\omega = [1, 2, 0]^T\] \[\omega \times (I\omega) = [1,1,0] \times [1,2,0] = [0, 0, 1]^T\] \[\dot{\omega} = I^{-1}(0 - [0,0,1]) = [0, 0, -\tfrac{1}{3}]^T \,\text{rad/s}^2\]
The body accelerates about the z-axis despite no external torque — this is the gyroscopic coupling effect due to asymmetric inertia.
float.| Variant | Key Difference | Use Case |
|---|---|---|
| Newton-Euler (this) | Single rigid body, Cartesian space | Spacecraft attitude, single-body simulation |
| Recursive Newton-Euler (RNEA) | Multi-body recursive algorithm, \(O(n)\) | Robot inverse dynamics, real-time control |
| Euler-Lagrange | Generalized coordinates, joint space | Control design, analytical derivations |
| Spatial vector algebra | 6D twist/wrench representation | Compact multi-body formulations |
The Recursive Newton-Euler Algorithm (RNEA) is the most efficient method for computing inverse dynamics of serial kinematic chains (robot arms, manipulators). Given joint positions, velocities, and desired accelerations, it computes the required joint torques in \(O(n)\) time — linear in the number of links.
This contrasts with evaluating the Euler-Lagrange equations directly: a straightforward Lagrangian evaluation grows as \(O(n^4)\), and even with a precomputed mass matrix the product \(M(q)\ddot{q}\) alone is \(O(n^2)\). Hollerbach’s comparative study (1980) puts the recursive Newton-Euler formulation at \(150n - 48\) multiplications and \(131n - 48\) additions — 852 and 738 for a 6-DOF arm — against tens of thousands of multiplications for the direct Lagrangian form.
The algorithm has two passes over the kinematic chain:
The joint torque at each joint is then the projection of the net torque onto the joint axis.
A serial chain of \(n\) rigid bodies (links) connected by revolute joints. Each link \(i\) is described by:
The rotation matrix \(R_i(q_i)\) transforms vectors from link \(i\) frame to parent frame, computed via Rodrigues’ formula from the joint angle \(q_i\) and joint axis \(\hat{z}_i\).
For link \(i\), given parent link \(i-1\) quantities:
Angular velocity: \[\omega_i = R_i^T \omega_{i-1} + \dot{q}_i \hat{z}_i\]
Angular acceleration: \[\dot{\omega}_i = R_i^T \dot{\omega}_{i-1} + \ddot{q}_i \hat{z}_i + \omega_i \times (\dot{q}_i \hat{z}_i)\]
Linear acceleration at joint origin of link \(i\): \[a_{J_i} = R_i^T \left( a_{J_{i-1}} + \dot{\omega}_{i-1} \times r_{p \to j}^{(i)} + \omega_{i-1} \times (\omega_{i-1} \times r_{p \to j}^{(i)}) \right)\]
Linear acceleration at center of mass: \[a_{C_i} = a_{J_i} + \dot{\omega}_i \times r_{j \to c}^{(i)} + \omega_i \times (\omega_i \times r_{j \to c}^{(i)})\]
For the base link (\(i = 0\)), the “parent” is the world frame with \(\omega_{-1} = 0\) and \(a_{J_{-1}} = -g\) (gravity expressed as a base acceleration, a standard RNEA convention).
For each link \(i\), starting from the tip:
Net force on link \(i\): \[f_i = m_i a_{C_i} + \sum_{\text{children } j} R_j f_j\]
Net torque on link \(i\) (about joint): \[\tau_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i) + r_{j \to c}^{(i)} \times (m_i a_{C_i}) + \sum_{\text{children } j} \left( R_j \tau_j + r_{p \to j}^{(j)} \times R_j f_j \right)\]
Joint torque: \[\tau_{q_i} = \hat{z}_i^T \tau_i\]
The rotation matrix for angle \(\theta\) about unit axis \(\hat{u} = [u_x, u_y, u_z]^T\):
\[R = \cos\theta \cdot \mathbf{I}_3 + (1 - \cos\theta) \hat{u}\hat{u}^T + \sin\theta [\hat{u}]_\times\]
where \([\hat{u}]_\times\) is the skew-symmetric matrix of \(\hat{u}\).
| Operation | Time | Space | Notes |
|---|---|---|---|
| Inverse dynamics | \(O(n)\) | \(O(n)\) | Two linear passes over \(n\) links |
| Per-link forward pass | \(O(1)\) | \(O(1)\) | Fixed 3×3 matrix-vector operations |
| Per-link backward pass | \(O(1)\) | \(O(1)\) | Cross products and additions |
| Rotation matrix | \(O(1)\) | \(O(1)\) | Rodrigues’ formula: trig functions + 3×3 matrix |
Compared to evaluating inverse dynamics through an explicit mass matrix (\(O(n^2)\) to form and apply) or a direct Lagrangian evaluation (\(O(n^4)\)), RNEA’s linear cost dominates for every practical \(n\); for \(n = 6\) it needs 852 multiplications and 738 additions (Hollerbach, 1980).
A single thin rod of mass \(m = 2\,\text{kg}\) and length \(l = 1\,\text{m}\), rotating about the z-axis. Joint at origin, CoM at \([l/2, 0, 0]^T\).
Inertia at CoM: \(I = ml^2/12 = 0.167\,\text{kg·m}^2\) (about y and z axes)
Setup: \(q = 0\), \(\dot{q} = 0\), \(\ddot{q} = 1\,\text{rad/s}^2\), zero gravity.
Forward pass: - \(\omega_0 = [0, 0, 0]^T + 0 \cdot [0, 0, 1]^T = [0, 0, 0]^T\) - \(\dot{\omega}_0 = [0, 0, 1]^T\) (from \(\ddot{q}\)) - \(a_{C_0} = 0 + [0, 0, 1] \times [0.5, 0, 0] + 0 = [0, 0.5, 0]^T\,\text{m/s}^2\)
Backward pass: - \(f_0 = 2 \cdot [0, 0.5, 0]^T = [0, 1, 0]^T\,\text{N}\) - \(\tau_0 = I \cdot [0, 0, 1]^T + 0 + [0.5, 0, 0] \times [0, 1, 0]^T\) - \(= [0, 0, 0.167]^T + [0, 0, 0.5]^T = [0, 0, 0.667]^T\) - \(\tau_q = [0, 0, 1]^T \cdot [0, 0, 0.667]^T = 0.667\,\text{N·m}\)
This matches \(I_{\text{end}} \cdot \ddot{q} = \frac{ml^2}{3} \cdot 1 = 0.667\).
float.| Variant | Key Difference | Use Case |
|---|---|---|
| RNEA (this) | \(O(n)\) inverse dynamics, serial chain | Real-time robot control, computed torque |
| RNEA + mass matrix | Call RNEA \(n+1\) times to build \(M(q)\) | Forward dynamics via \(\ddot{q} = M^{-1}(\tau - h)\) |
| Articulated Body Algorithm (ABA) | \(O(n)\) forward dynamics directly | Simulation without forming \(M\) |
| Extended RNEA | Includes friction, motor inertia | Realistic robot modeling |
| RNEA for trees | Handles branching chains | Humanoid robots, multi-fingered hands |