Dynamics of Open Chains

Textbook Chapter 8

Robot convenience wrappers

For simplified calls using a Robot model (e.g., inverse_dynamics_rnea(robot, q, dq, ddq)), see Robot Model.

ModernRoboticsBook.ad — Method
ad(V)

Computes the $6 \times 6$ matrix $[\text{ad}_V]$ used to calculate the Lie bracket $[V_1, V_2] = [\text{ad}_{V_1}] V_2$.

What is the Lie bracket?

The Lie bracket measures how two twists interact — it captures the velocity produced by the non-commutativity of two motions. In dynamics, the $[\text{ad}]$ matrix appears in the recursive Newton-Euler algorithm, where it accounts for how the velocity of one link affects the forces on adjacent links through Coriolis and centripetal effects.

Arguments

  • V: a 6-vector spatial velocity (twist).

Returns

The $6 \times 6$ matrix $[\text{ad}_V]$.

Examples

julia> ad([1, 2, 3, 4, 5, 6])
6×6 StaticArraysCore.SMatrix{6, 6, Int64, 36} with indices SOneTo(6)×SOneTo(6):
  0  -3   2   0   0   0
  3   0  -1   0   0   0
 -2   1   0   0   0   0
  0  -6   5   0  -3   2
  6   0  -4   3   0  -1
 -5   4   0  -2   1   0
source
ModernRoboticsBook.end_effector_forces — Method
end_effector_forces(joint_positions, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes the joint forces/torques an open chain robot requires only to create the end-effector force tip_wrench.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • tip_wrench: the spatial force applied by the end-effector expressed in frame {n+1}.
  • link_frames: the list of link frames i relative to i-1 at the home position.
  • spatial_inertias: the spatial inertia matrices Gi of the links.
  • screw_axes: the screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns.

Returns the joint forces and torques required only to create the end-effector force tip_wrench. This function calls inversedynamicsrnea with gravity = 0, joint_velocities = 0, and joint_accelerations = 0.

Examples

julia> import LinearAlgebra as LA

julia> joint_positions = [0.1, 0.1, 0.1]
3-element Vector{Float64}:
 0.1
 0.1
 0.1

julia> tip_wrench = [1, 1, 1, 1, 1, 1]
6-element Vector{Int64}:
 1
 1
 1
 1
 1
 1

julia> M01 = [1 0 0        0;
              0 1 0        0;
              0 0 1 0.089159;
              0 0 0        1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0  0.0
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  0.089159
 0.0  0.0  0.0  1.0

julia> M12 = [ 0 0 1    0.28;
               0 1 0 0.13585;
              -1 0 0       0;
               0 0 0       1]
4×4 Matrix{Float64}:
  0.0  0.0  1.0  0.28
  0.0  1.0  0.0  0.13585
 -1.0  0.0  0.0  0.0
  0.0  0.0  0.0  1.0

julia> M23 = [1 0 0       0;
              0 1 0 -0.1197;
              0 0 1   0.395;
              0 0 0       1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0   0.0
 0.0  1.0  0.0  -0.1197
 0.0  0.0  1.0   0.395
 0.0  0.0  0.0   1.0

julia> M34 = [1 0 0       0;
              0 1 0       0;
              0 0 1 0.14225;
              0 0 0       1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0  0.0
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  0.14225
 0.0  0.0  0.0  1.0

julia> link_frames = [M01, M12, M23, M34]
4-element Vector{Matrix{Float64}}:
 [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0; 0.0 0.0 1.0 0.089159; 0.0 0.0 0.0 1.0]
 [0.0 0.0 1.0 0.28; 0.0 1.0 0.0 0.13585; -1.0 0.0 0.0 0.0; 0.0 0.0 0.0 1.0]
 [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 -0.1197; 0.0 0.0 1.0 0.395; 0.0 0.0 0.0 1.0]
 [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0; 0.0 0.0 1.0 0.14225; 0.0 0.0 0.0 1.0]

julia> G1 = LA.Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])
6×6 LinearAlgebra.Diagonal{Float64, Vector{Float64}}:
 0.010267   ⋅         ⋅        ⋅    ⋅    ⋅ 
  ⋅        0.010267   ⋅        ⋅    ⋅    ⋅ 
  ⋅         ⋅        0.00666   ⋅    ⋅    ⋅ 
  ⋅         ⋅         ⋅       3.7   ⋅    ⋅ 
  ⋅         ⋅         ⋅        ⋅   3.7   ⋅ 
  ⋅         ⋅         ⋅        ⋅    ⋅   3.7

julia> G2 = LA.Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])
6×6 LinearAlgebra.Diagonal{Float64, Vector{Float64}}:
 0.22689   ⋅        ⋅          ⋅      ⋅      ⋅ 
  ⋅       0.22689   ⋅          ⋅      ⋅      ⋅ 
  ⋅        ⋅       0.0151074   ⋅      ⋅      ⋅ 
  ⋅        ⋅        ⋅         8.393   ⋅      ⋅ 
  ⋅        ⋅        ⋅          ⋅     8.393   ⋅ 
  ⋅        ⋅        ⋅          ⋅      ⋅     8.393

julia> G3 = LA.Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])
6×6 LinearAlgebra.Diagonal{Float64, Vector{Float64}}:
 0.0494433   ⋅          ⋅         ⋅      ⋅      ⋅ 
  ⋅         0.0494433   ⋅         ⋅      ⋅      ⋅ 
  ⋅          ⋅         0.004095   ⋅      ⋅      ⋅ 
  ⋅          ⋅          ⋅        2.275   ⋅      ⋅ 
  ⋅          ⋅          ⋅         ⋅     2.275   ⋅ 
  ⋅          ⋅          ⋅         ⋅      ⋅     2.275

julia> spatial_inertias = [G1, G2, G3]
3-element Vector{LinearAlgebra.Diagonal{Float64, Vector{Float64}}}:
 Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])
 Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])
 Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])

julia> screw_axes = [ 1  0  1      0  1      0;
                      0  1  0 -0.089  0      0;
                      0  1  0 -0.089  0  0.425]'
6×3 adjoint(::Matrix{Float64}) with eltype Float64:
 1.0   0.0     0.0
 0.0   1.0     1.0
 1.0   0.0     0.0
 0.0  -0.089  -0.089
 1.0   0.0     0.0
 0.0   0.0     0.425

julia> end_effector_forces(joint_positions, tip_wrench, link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
 1.4095460782639782
 1.8577149723180628
 1.392409
source
ModernRoboticsBook.euler_step — Method
euler_step(joint_positions, joint_velocities, joint_accelerations, timestep)

Compute the joint angles and velocities at the next timestep using first order Euler integration.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • joint_velocities: the $n$-vector of joint rates.
  • joint_accelerations: the $n$-vector of joint accelerations.
  • timestep: the timestep delta t.

Return

  • joint_positionsNext: the vector of joint variables after timestep from first order Euler integration.
  • joint_velocitiesNext: the vector of joint rates after timestep from first order Euler integration.

Examples

julia> euler_step([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [2, 1.5, 1], 0.1)
([0.11000000000000001, 0.12000000000000001, 0.13], [0.30000000000000004, 0.35000000000000003, 0.4])
source
ModernRoboticsBook.forward_dynamics_aba! — Method
forward_dynamics_aba!(ddq, joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes, Ai, AdTi, Vi, ci, IA, pA, U, D, u)

In-place version of forward_dynamics_aba. Writes the result into ddq. The workspace vectors must be pre-allocated: Ai (n), AdTi (n+1), Vi (n+1), ci (n), IA (n), pA (n), U (n), D (n), u (n).

source
ModernRoboticsBook.forward_dynamics_aba — Method
forward_dynamics_aba(joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes forward dynamics using the Articulated Body Algorithm (ABA).

Articulated Body Algorithm

ABA solves for joint accelerations in O(n) with three passes over the kinematic chain, without forming or inverting the mass matrix:

  1. Outward pass: compute link velocities and velocity-dependent bias forces.
  2. Inward pass: accumulate articulated body inertias — the effective inertia of each subtree accounting for joint freedom — from leaf to root.
  3. Outward pass: solve for joint accelerations from root to leaf.

This is the same algorithm Pinocchio uses for forward dynamics.

Educational note

The textbook (Chapter 8.3) computes forward dynamics by explicitly forming $M(\theta)$ and solving $M \ddot{\theta} = \tau - c - g$. See forward_dynamics_crba for that approach. ABA avoids forming $M$ entirely, which is asymptotically faster for large $n$.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • joint_velocities: the $n$-vector of joint rates.
  • joint_torques: the $n$-vector of joint forces/torques.
  • gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
  • tip_wrench: the wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of joint accelerations $\ddot{\theta}$.

source
ModernRoboticsBook.forward_dynamics_crba! — Method
forward_dynamics_crba!(ddq, joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes, M, tau_bias, Ai, AdTi, Gc, Vi, Vdi)

In-place version of forward_dynamics_crba. Writes the result into ddq. Workspace: M (n×n), tau_bias (n), Ai (n), AdTi (n+1), Gc (n), Vi (n+1), Vdi (n+1).

source
ModernRoboticsBook.forward_dynamics_crba — Method
forward_dynamics_crba(joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes forward dynamics in the space frame for an open chain robot.

How does forward dynamics work?

Forward dynamics answers: "given the applied torques, how does the robot accelerate?" It solves the manipulator equation for joint accelerations:

\[\ddot{\theta} = M(\theta)^{-1} \left[ \tau - c(\theta,\dot{\theta}) - g(\theta) - J^T(\theta) \mathcal{F}_{\text{tip}} \right]\]

where $M$ is the mass matrix (computed via CRBA), and the right-hand side is computed via a single inverse_dynamics_rnea call with zero accelerations that captures velocity-dependent, gravitational, and tip wrench forces.

Educational note

The textbook computes forward dynamics by explicitly forming $M^{-1}$ and calling inverse_dynamics_rnea separately for Coriolis, gravity, and tip wrench terms. This implementation makes two improvements:

  1. Single RNEA call: instead of computing $c$, $g$, and $J^T F$ separately, a single call to RNEA with zero accelerations returns $\tau_{\text{bias}} = c + g + J^T F$.
  2. Backslash solve (M \ b): instead of explicitly forming $M^{-1}$ and multiplying, Julia's \ operator solves the linear system $M x = b$ via LU factorization. This is both faster (avoids forming the inverse) and more numerically stable (fewer floating-point operations, better conditioning).

For the fastest forward dynamics, use forward_dynamics_aba which avoids forming $M$ entirely. See forward_dynamics_rnea for the textbook algorithm.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • joint_velocities: the $n$-vector of joint rates.
  • joint_torques: the $n$-vector of joint forces/torques.
  • gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
  • tip_wrench: the wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of joint accelerations $\ddot{\theta}$.

Examples

julia> forward_dynamics_crba([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [0.5, 0.6, 0.7], [0, 0, -9.8], [1, 1, 1, 1, 1, 1], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  -0.9739290670855625
  25.584667840340547
 -32.91499212478147
source
ModernRoboticsBook.forward_dynamics_rnea! — Method
forward_dynamics_rnea!(ddq, joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes, M, tau_c, tau_g, tau_f, Ai, AdTi, Gc, Vi, Vdi)

In-place version of forward_dynamics_rnea. Writes the result into ddq. Workspace: M (n×n), tau_c (n), tau_g (n), tau_f (n), Ai (n), AdTi (n+1), Gc (n), Vi (n+1), Vdi (n+1).

source
ModernRoboticsBook.forward_dynamics_rnea — Method
forward_dynamics_rnea(joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes forward dynamics using the textbook algorithm (Chapter 8.3): explicitly forms $M^{-1}$ and calls inverse_dynamics_rnea separately for Coriolis, gravity, and tip wrench terms. This is slower than forward_dynamics_crba (which uses CRBA + single RNEA + backslash) but directly mirrors the textbook equation $\ddot{\theta} = M^{-1}[\tau - c - g - J^T F_{\text{tip}}]$.

source
ModernRoboticsBook.forward_dynamics_trajectory — Method
forward_dynamics_trajectory(joint_positions, joint_velocities, joint_torque_traj, gravity, tip_wrench_traj, link_frames, spatial_inertias, screw_axes, timestep, integration_resolution)

Simulates the motion of a serial chain given an open-loop history of joint forces/torques. Uses forward_dynamics_crba with euler_step integration at each timestep.

Arguments

  • joint_positions: the $n$-vector of initial joint variables.
  • joint_velocities: the $n$-vector of initial joint velocities.
  • joint_torque_traj: an $N×n$ matrix where row $i$ is the joint force/torque vector at timestep $i$.
  • gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
  • tip_wrench_traj: an $N×6$ matrix where row $i$ is the spatial wrench applied by the end-effector at timestep $i$.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.
  • timestep: the timestep $\Delta t$ between consecutive trajectory points.
  • integration_resolution: the number of Euler integration steps during each timestep $\Delta t$. Must be a positive integer $\geq 1$. Larger values result in slower simulations but less accumulation of integration error.

Returns

  • joint_position_traj: the $N×n$ matrix of joint variables resulting from the specified joint forces/torques.
  • joint_velocity_traj: the $N×n$ matrix of joint velocities resulting from the specified joint forces/torques.

Examples

julia> joint_torque_traj = [3.63 -6.58 -5.57; 3.74 -5.55 -5.5; 4.31 -0.68 -5.19];

julia> thetamat, dthetamat = forward_dynamics_trajectory([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], joint_torque_traj, [0, 0, -9.8], ones(3, 6), link_frames, spatial_inertias, screw_axes, 0.1, 8);

julia> thetamat
3×3 adjoint(::Matrix{Float64}) with eltype Float64:
 0.1       0.1        0.1
 0.106431  0.2626    -0.226649
 0.10198   0.715813  -1.22522
source
ModernRoboticsBook.gravity_forces — Method
gravity_forces(joint_positions, gravity, link_frames, spatial_inertias, screw_axes)

Computes the joint forces/torques an open chain robot requires to overcome gravity at its configuration. Calls inverse_dynamics_rnea with joint_velocities = 0, joint_accelerations = 0, and tip_wrench = 0.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of joint gravity torques $g(\theta)$.

Examples

julia> gravity_forces([0.1, 0.1, 0.1], [0, 0, -9.8], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  28.40331261821983
 -37.64094817177068
  -5.4415891999683605
source
ModernRoboticsBook.inverse_dynamics_rnea! — Method
inverse_dynamics_rnea!(joint_torques, joint_positions, joint_velocities, joint_accelerations, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes, Ai, AdTi, Vi, Vdi)

In-place version of inverse_dynamics_rnea. Writes the result into joint_torques. The workspace vectors Ai (length n), AdTi (length n+1), Vi (length n+1), and Vdi (length n+1) must be pre-allocated.

source
ModernRoboticsBook.inverse_dynamics_rnea — Method
inverse_dynamics_rnea(joint_positions, joint_velocities, joint_accelerations, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes inverse dynamics in the space frame for an open chain robot using forward-backward Newton-Euler iterations.

Recursive Newton-Euler algorithm

Inverse dynamics answers: "what joint torques do I need to produce a desired motion?" It uses the recursive Newton-Euler algorithm, which works in two passes:

  1. Forward pass (base → tip): propagate link velocities and accelerations outward along the chain, computing each link's twist and acceleration from its parent's motion plus the joint's contribution.
  2. Backward pass (tip → base): propagate wrenches inward, computing the force each link needs (from its inertia and acceleration) and projecting onto joint axes to get the required torque.

This is $O(n)$ in the number of joints — much faster than forming and inverting the full dynamics equation. It is also the workhorse behind most dynamics computations: mass_matrix_crba, velocity_quadratic_forces, gravity_forces, and end_effector_forces are all computed by calling this function with specific inputs zeroed out.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • joint_velocities: the $n$-vector of joint rates.
  • joint_accelerations: the $n$-vector of joint accelerations.
  • gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
  • tip_wrench: the wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$ (the end-effector frame relative to the last link).
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of required joint forces/torques.

Examples

julia> inverse_dynamics_rnea([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [2, 1.5, 1], [0, 0, -9.8], [1, 1, 1, 1, 1, 1], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  74.6961615528745
 -33.067660158514585
  -3.2305731379014246
source
ModernRoboticsBook.inverse_dynamics_trajectory — Method
inverse_dynamics_trajectory(joint_position_traj, joint_velocity_traj, joint_acceleration_traj, gravity, tip_wrench_traj, link_frames, spatial_inertias, screw_axes)

Calculates the joint forces/torques required to move the serial chain along the given trajectory using inverse_dynamics_rnea at each timestep.

Arguments

  • joint_position_traj: an $N×n$ matrix of joint variables, where row $i$ is the joint vector at timestep $i$.
  • joint_velocity_traj: an $N×n$ matrix of joint velocities.
  • joint_acceleration_traj: an $N×n$ matrix of joint accelerations.
  • gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
  • tip_wrench_traj: an $N×6$ matrix where row $i$ is the spatial wrench applied by the end-effector at timestep $i$.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

An $N×n$ matrix of joint forces/torques, where row $i$ is the joint force-torque at timestep $i$.

Examples

julia> traj_pos = [0.0 0.0 0.0; 0.5 0.5 0.5; 1.0 1.0 1.0];

julia> traj_vel = [0.0 0.0 0.0; 0.5 0.5 0.5; 1.0 1.0 1.0];

julia> traj_acc = [0.5 0.5 0.5; 0.5 0.5 0.5; 0.5 0.5 0.5];

julia> inverse_dynamics_trajectory(traj_pos, traj_vel, traj_acc, [0, 0, -9.8], ones(3, 6), link_frames, spatial_inertias, screw_axes)
3×3 adjoint(::Matrix{Float64}) with eltype Float64:
  23.7113  -35.2321  -3.87345
 105.336   -27.6441  -1.41169
 129.143   -17.3084   2.30991
source
ModernRoboticsBook.mass_matrix_crba! — Method
mass_matrix_crba!(M, joint_positions, link_frames, spatial_inertias, screw_axes, Ai, AdTi, Gc)

In-place version of mass_matrix_crba using the Composite Rigid Body Algorithm (CRBA). Writes the result into M. The workspace vectors Ai (length n), AdTi (length n+1), and Gc (length n) must be pre-allocated.

source
ModernRoboticsBook.mass_matrix_crba — Method
mass_matrix_crba(joint_positions, link_frames, spatial_inertias, screw_axes)

Computes the mass matrix $M(\theta)$ of an open chain robot using the Composite Rigid Body Algorithm (CRBA).

Composite Rigid Body Algorithm

The mass matrix $M(\theta)$ relates joint accelerations to joint torques in the absence of velocity-dependent and gravitational forces: $\tau = M(\theta) \ddot{\theta}$. CRBA computes $M$ in a single backward pass by accumulating composite spatial inertias — the total inertia of each link plus all its descendants — from leaf to root, then assembling the matrix entries by projecting these composite inertias onto the joint screw axes.

Educational note

The textbook (Chapter 8.2) teaches a simpler approach that calls inverse_dynamics_rnea $n$ times with unit accelerations: "what torques are needed for unit acceleration of joint $i$?" Each call produces one column of $M(\theta)$. This builds intuition but requires $n$ full Newton-Euler passes. CRBA computes the same result with one forward pass and one backward pass, exploiting the symmetry of $M$. The textbook algorithm is preserved See mass_matrix_rnea for this textbook algorithm.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n×n$ mass matrix $M(\theta)$.

Examples

julia> mass_matrix_crba([0.1, 0.1, 0.1], link_frames, spatial_inertias, screw_axes)
3×3 Matrix{Float64}:
 22.5433      -0.307147  -0.00718426
 -0.307147     1.96851    0.432157
 -0.00718426   0.432157   0.191631
source
ModernRoboticsBook.mass_matrix_rnea — Method
mass_matrix_rnea(joint_positions, link_frames, spatial_inertias, screw_axes)

Computes the mass matrix using the textbook algorithm (Chapter 8.2): calls inverse_dynamics_rnea $n$ times, each with a unit acceleration vector. This is slower than mass_matrix_crba (which uses CRBA) but directly illustrates that column $i$ of $M(\theta)$ is the torque needed for unit acceleration of joint $i$.

source
ModernRoboticsBook.velocity_quadratic_forces — Method
velocity_quadratic_forces(joint_positions, joint_velocities, link_frames, spatial_inertias, screw_axes)

Computes the Coriolis and centripetal terms $c(\theta, \dot{\theta})$ in the inverse dynamics of an open chain robot. Calls inverse_dynamics_rnea with gravity = 0, tip_wrench = 0, and joint_accelerations = 0.

Arguments

  • joint_positions: the $n$-vector of joint variables.
  • joint_velocities: the $n$-vector of joint rates.
  • link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
  • spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
  • screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of Coriolis and centripetal terms.

Examples

julia> velocity_quadratic_forces([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  0.26453118054501235
 -0.0550515682891655
 -0.006891320068248912
source