A Field Guide to Embedded Robot Manipulator Kinematics, Dynamics, and Control
August 01, 2026
Forward Kinematics computes the 3D Cartesian positions of all joints in a serial kinematic chain, given the joint angles. For a chain of \(n\) revolute joints, it produces \(n + 1\) position vectors (base through end-effector) in \(O(n)\) time.
This is the geometric foundation for visualization, collision detection, and workspace analysis. It answers the question: “given these joint angles, where is each joint in world space?”
Starting from the base at the origin, the position of joint \(i + 1\) is:
\[p_{i+1} = p_i + R_{\text{world} \to i} \cdot r_{i \to i+1}\]
where:
Each rotation \(R_k(q_k)\) is computed via Rodrigues’ formula from the joint axis and angle.
For the last link, the offset to the end-effector is approximated as \(2 \cdot r_{j \to \text{CoM}}\) (assuming the center of mass is at the midpoint of the link).
| Case | Time | Space | Notes |
|---|---|---|---|
| All | \(O(n)\) | \(O(n)\) | Single pass over chain |
Consider a 2-link planar arm with link lengths \(L_1 = 1\), \(L_2 = 0.8\), Y-axis joints, and \(q = [\pi/4, -\pi/6]\).
parentToJoint
and jointToCoM are both zero, consecutive joints collapse
to the same position.jointToCoM * 2, which is exact only for
uniform-density links with the CoM at the geometric center.math::RotationAboutAxis from Geometry3D.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 end-effector position
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
GaussianElimination<T, 3> 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\|}{\lambda}\]
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.
| 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 ~40 iterations \(q\) converges to joints that produce \(p_{\text{ee}} \approx (0.5, 1.0, 0)\).
converged = false; the end-effector
will be as close as geometrically possible.std::cos,
std::sin, and std::sqrt; Q15/Q31 fixed-point
types are not supported.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.numerical/math/Geometry3D.hpp: Provides
CrossProduct for Jacobian column computation,
RotationAboutAxis for accumulated rotation, and
VectorNorm for convergence check.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.
Alternatives like Euler-Lagrange require explicitly forming and inverting the \(n \times n\) mass matrix (\(O(n^3)\)), making ABA significantly faster for chains with more than 3-4 links.
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\) is the spatial acceleration contribution from the parent.
| 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 planar arm with unit masses, unit lengths, Y-axis joints, 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: both joints accelerate downward due to gravity, with the base joint carrying the larger moment from the full chain.
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 the Euler-Lagrange approach, which requires explicitly forming and multiplying \(n \times n\) matrices (\(O(n^3)\)). For a 6-DOF robot arm, RNEA is roughly 10× faster than the matrix-based approach.
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 Euler-Lagrange \(O(n^3)\) inverse dynamics, RNEA is dramatically faster for large \(n\). For \(n = 6\) (typical robot arm), RNEA performs roughly 780 floating-point operations vs. ~14,000 for the matrix approach.
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 |