In earlier chapters, we modeled manipulators whose base was rigidly attached to the environment (fixed-base). For legged, humanoid, or free-flying robots, the base itself moves in space. The configuration space is then the product of a rigid-body pose and internal joint coordinates.

Let the floating base be represented by a homogeneous transform \( {}^{W}\mathbf{T}_B \in SE(3) \), mapping points from the base frame \( B \) to the world frame \( W \). Using a rotation matrix \( {}^{W}\mathbf{R}_B \in SO(3) \) and position \( {}^{W}\mathbf{p}_B \in \mathbb{R}^3 \):

\[ {}^{W}\mathbf{T}_B = \begin{bmatrix} {}^{W}\mathbf{R}_B & {}^{W}\mathbf{p}_B \\ \mathbf{0}_{1\times 3} & 1 \end{bmatrix}. \]

For a robot with \( n \) joints, the configuration manifold is

\[ \mathcal{Q} = SE(3) \times \mathbb{R}^{n}. \]

A corresponding coordinate vector (not unique) can be written as

\[ \mathbf{q} = \begin{bmatrix} \mathbf{q}_B \\ \mathbf{q}_J \end{bmatrix} \in \mathbb{R}^{n+6}, \quad \mathbf{q}_B = \begin{bmatrix} {}^{W}\mathbf{p}_B \\ \boldsymbol{\phi}_B \end{bmatrix}, \]

where \( \boldsymbol{\phi}_B \in \mathbb{R}^3 \) is a minimal orientation description (for instance exponential coordinates introduced in the earlier Lie-group chapter). The joint coordinates \( \mathbf{q}_J \in \mathbb{R}^n \) are the usual revolute/prismatic joint variables.

flowchart TD
  W["World frame W"] --> B["Floating base frame B"]
  B --> J1["Joint coordinates q_J"]
  J1 --> CFG["Configuration q = (q_B, q_J)"]
  CFG --> KIN["Forward kinematics of all links"]
  KIN --> DYN["Kinetic & potential energy of multi-body system"]
        

As in fixed-base modeling, all link poses and velocities will be expressed as functions of \( \mathbf{q} \) and its time derivative, but now the base pose \( {}^{W}\mathbf{T}_B \) is itself a configuration variable rather than a constant.

There are multiple common ways to parametrize the base pose. Two broad categories:

• Minimal coordinates: position \( {}^{W}\mathbf{p}_B \in \mathbb{R}^3 \) and a minimal orientation vector \( \boldsymbol{\phi}_B \in \mathbb{R}^3 \) (e.g. Euler angles, exponential coordinates).
• Redundant coordinates with constraints: position \( {}^{W}\mathbf{p}_B \in \mathbb{R}^3 \) and a unit quaternion \( \mathbf{q}_B^{\text{quat}} \in \mathbb{R}^4 \) with constraint \( \| \mathbf{q}_B^{\text{quat}} \| = 1 \).

In this course, to keep consistency with SE(3) and twist formulations from previous chapters, we conceptually treat the base orientation as an element of \( SO(3) \), and only use a local coordinate \( \boldsymbol{\phi}_B \) when necessary. The configuration vector (minimal) is then

\[ \mathbf{q}_B = \begin{bmatrix} {}^{W}\mathbf{p}_B \\ \boldsymbol{\phi}_B \end{bmatrix} \in \mathbb{R}^6, \quad \mathbf{q} = \begin{bmatrix} \mathbf{q}_B \\ \mathbf{q}_J \end{bmatrix} \in \mathbb{R}^{n+6}. \]

The total degrees of freedom (DoFs) are thus \( n+6 \). The first 6 DoFs correspond to the base translation and orientation; crucially, these are unactuated in most legged robots (no thrusters acting directly on the base), so the associated generalized forces will be identically zero in the equations of motion.

When using non-minimal coordinates (e.g. quaternions), the configuration space is embedded into a higher-dimensional Euclidean space with holonomic constraints. This point of view connects floating-base modeling directly to the constrained dynamics formulation you saw in the chapter on holonomic constraints.

Recall from earlier chapters that the motion of a rigid body in 3D can be described by a spatial twist \( {}^{W}\mathbf{V} \in \mathbb{R}^6 \), stacking angular and linear velocity about a frame origin:

\[ {}^{W}\mathbf{V}_B = \begin{bmatrix} {}^{W}\boldsymbol{\omega}_B \\ {}^{W}\mathbf{v}_B \end{bmatrix}. \]

Let \( \dot{\mathbf{q}}_B \in \mathbb{R}^6 \) denote the time derivative of the base coordinate vector \( \mathbf{q}_B \), and \( \dot{\mathbf{q}}_J \in \mathbb{R}^n \) the vector of joint velocities. In general, there exists a configuration-dependent mapping

\[ {}^{W}\mathbf{V}_B = \mathbf{J}_B(\mathbf{q}_B)\,\dot{\mathbf{q}}_B, \]

where \( \mathbf{J}_B(\mathbf{q}_B) \in \mathbb{R}^{6\times 6} \) is the base Jacobian relating minimal orientation rates to angular velocity. If we choose \( \mathbf{q}_B \) as exponential coordinates, this mapping is non-singular except at certain singular configurations.

For multi-body systems, we define the generalized velocity vector as

\[ \mathbf{v} = \begin{bmatrix} {}^{W}\mathbf{V}_B \\ \dot{\mathbf{q}}_J \end{bmatrix} \in \mathbb{R}^{n+6}. \]

This choice is particularly convenient in spatial-vector formulations (as in Featherstone’s approach), where the same dimension \( n+6 \) appears both in coordinates and generalized velocities, but the first 6 components are now twist components rather than raw orientation rates.

For each link \( i \), with spatial twist \( {}^{W}\mathbf{V}_i \in \mathbb{R}^6 \), there exists a geometric Jacobian \( \mathbf{J}_i(\mathbf{q}) \in \mathbb{R}^{6\times (n+6)} \) such that

\[ {}^{W}\mathbf{V}_i = \mathbf{J}_i(\mathbf{q})\,\mathbf{v}. \]

This extends the end-effector Jacobians from the differential kinematics chapter to all links in a floating-base tree: the first 6 columns correspond to the base motion, and the remaining \( n \) columns correspond to joint motions.

Let the kinetic energy of the multi-body system be expressed in terms of generalized velocities as

\[ T(\mathbf{q},\mathbf{v}) = \frac{1}{2}\,\mathbf{v}^{\mathsf{T}}\,\mathbf{H}(\mathbf{q})\,\mathbf{v}, \]

where \( \mathbf{H}(\mathbf{q}) \in \mathbb{R}^{(n+6)\times (n+6)} \) is the generalized inertia matrix for the floating-base robot. The potential energy \( V(\mathbf{q}) \) depends only on configuration. The Lagrangian is

\[ \mathcal{L}(\mathbf{q},\mathbf{v}) = T(\mathbf{q},\mathbf{v}) - V(\mathbf{q}). \]

The generalized momentum is defined as the gradient of the Lagrangian with respect to generalized velocity:

\[ \mathbf{p} = \frac{\partial \mathcal{L}}{\partial \mathbf{v}} \in \mathbb{R}^{n+6}. \]

Since \( V(\mathbf{q}) \) does not depend on \( \mathbf{v} \), we have

\[ \mathbf{p} = \frac{\partial T}{\partial \mathbf{v}} = \frac{\partial}{\partial \mathbf{v}} \left( \frac{1}{2}\,\mathbf{v}^{\mathsf{T}}\mathbf{H}(\mathbf{q})\mathbf{v} \right) = \mathbf{H}(\mathbf{q})\,\mathbf{v}. \]

Proof sketch: For any symmetric matrix \( \mathbf{H} \), the quadratic form \( f(\mathbf{v}) = \tfrac{1}{2}\mathbf{v}^{\mathsf{T}} \mathbf{H}\mathbf{v} \) satisfies \( \partial f / \partial \mathbf{v} = \mathbf{H}\mathbf{v} \) by standard multivariable calculus. Symmetry of \( \mathbf{H}(\mathbf{q}) \) is guaranteed by the kinetic energy construction (Chapter 13).

Therefore, generalized momentum provides a compact representation of the system's linear and angular momentum (embedded in the first 6 entries) and the momentum associated with joint coordinates (remaining entries).

flowchart TD
  QV["Inputs: q, v"] --> H["Build inertia matrix H(q)"]
  H --> P["Compute generalized momentum p = H(q) v"]
  P --> EOM["Use p in Euler-Lagrange equations"]
  EOM --> ANA["Analyze conservation and coupling effects"]
        

It is extremely useful to partition the inertia matrix and generalized velocity into base and joint parts. Write

\[ \mathbf{v} = \begin{bmatrix} {}^{W}\mathbf{V}_B \\ \dot{\mathbf{q}}_J \end{bmatrix}, \quad \mathbf{H}(\mathbf{q}) = \begin{bmatrix} \mathbf{H}_{BB}(\mathbf{q}) & \mathbf{H}_{BJ}(\mathbf{q}) \\ \mathbf{H}_{JB}(\mathbf{q}) & \mathbf{H}_{JJ}(\mathbf{q}) \end{bmatrix}. \]

Then

\[ \mathbf{p} = \begin{bmatrix} \mathbf{p}_B \\ \mathbf{p}_J \end{bmatrix} = \begin{bmatrix} \mathbf{H}_{BB} & \mathbf{H}_{BJ} \\ \mathbf{H}_{JB} & \mathbf{H}_{JJ} \end{bmatrix} \begin{bmatrix} {}^{W}\mathbf{V}_B \\ \dot{\mathbf{q}}_J \end{bmatrix}. \]

Explicitly,

\[ \mathbf{p}_B = \mathbf{H}_{BB}(\mathbf{q})\,{}^{W}\mathbf{V}_B + \mathbf{H}_{BJ}(\mathbf{q})\,\dot{\mathbf{q}}_J, \quad \mathbf{p}_J = \mathbf{H}_{JB}(\mathbf{q})\,{}^{W}\mathbf{V}_B + \mathbf{H}_{JJ}(\mathbf{q})\,\dot{\mathbf{q}}_J. \]

The block \( \mathbf{H}_{BB} \) acts as the composite spatial inertia of all links expressed about the base frame; the off-diagonal blocks \( \mathbf{H}_{BJ}, \mathbf{H}_{JB} \) describe how joint motion contributes to base momentum and conversely how base motion affects joint momenta.

The top 6 components \( \mathbf{p}_B \) are closely related to the total linear and angular momentum of the entire system expressed at the base frame. When expressed at the system center of mass (COM), they lead to the centroidal momentum, which will be developed rigorously in a later lesson of this chapter.

In the absence of external wrenches (true free-floating motion), the Newton-Euler equations imply that the total spatial momentum in an inertial frame is conserved. In this representation, that conservation appears as invariance of a suitable linear combination of the components of \( \mathbf{p} \).

Before tackling full 3D multi-body systems, consider a single rigid body moving in the plane (e.g. a satellite constrained to a plane). Its configuration is \( \mathbf{q} = [x,\,y,\,\theta]^{\mathsf{T}} \), where \( x,y \) are the coordinates of the center of mass (COM) in the world frame and \( \theta \) is the orientation about the out-of-plane axis.

Let mass be \( m \) and planar moment of inertia about COM be \( I \). The velocity vector is \( \dot{\mathbf{q}} = [\dot{x},\,\dot{y},\,\dot{\theta}]^{\mathsf{T}} \). The kinetic energy is

\[ T = \frac{1}{2}m(\dot{x}^2 + \dot{y}^2) + \frac{1}{2}I\,\dot{\theta}^2. \]

This can be written in matrix form as

\[ T = \frac{1}{2} \begin{bmatrix} \dot{x} & \dot{y} & \dot{\theta} \end{bmatrix} \begin{bmatrix} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & I \end{bmatrix} \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \frac{1}{2}\,\dot{\mathbf{q}}^{\mathsf{T}} \mathbf{H}\,\dot{\mathbf{q}}. \]

Thus the inertia matrix is diagonal,

\[ \mathbf{H} = \begin{bmatrix} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & I \end{bmatrix}, \quad \mathbf{p} = \mathbf{H}\dot{\mathbf{q}} = \begin{bmatrix} m\dot{x} \\ m\dot{y} \\ I\dot{\theta} \end{bmatrix}. \]

The generalized momentum components are directly the linear momentum in the \( x \) and \( y \) directions and the angular momentum about the COM. This single-body example generalizes to multi-body systems via the composite rigid-body construction of \( \mathbf{H}(\mathbf{q}) \).

The Euler-Lagrange equations for each generalized coordinate \( q_i \) read

\[ \frac{\mathrm{d}}{\mathrm{d}t} \left( \frac{\partial \mathcal{L}}{\partial v_i} \right) - \frac{\partial \mathcal{L}}{\partial q_i} = Q_i, \]

where \( Q_i \) is the generalized non-conservative force (e.g. actuator torque). In vector form, using \( \mathbf{p} = \mathbf{H}(\mathbf{q})\mathbf{v} \), one can show that the equations of motion can be written as

\[ \mathbf{H}(\mathbf{q})\,\dot{\mathbf{v}} + \mathbf{C}(\mathbf{q},\mathbf{v})\,\mathbf{v} + \mathbf{g}(\mathbf{q}) = \mathbf{Q}, \]

where \( \mathbf{C}(\mathbf{q},\mathbf{v}) \) is the Coriolis/centrifugal matrix and \( \mathbf{g}(\mathbf{q}) \) the gravity vector, extending the familiar manipulator equation to \( n+6 \) coordinates.

For a floating-base robot with actuators only at the joints, the generalized force vector has the structure

\[ \mathbf{Q} = \begin{bmatrix} \mathbf{0}_{6} \\ \boldsymbol{\tau} \end{bmatrix}, \]

where \tau denotes the joint torque vector (dimension \( n \)). This reflects that no direct actuation acts on the base (ignoring thrusters or reaction wheels). External wrenches from contacts can be included on the right-hand side as additional generalized forces derived via Jacobians, but detailed contact modeling is deferred to a later lesson.

The first six equations correspond to the global Newton-Euler balance for the whole robot, while the remaining \( n \) equations correspond to joint dynamics.

We illustrate how to represent floating-base state vectors and compute generalized momentum using Python and NumPy. In practice, a dedicated robotics library such as pinocchio or robotics-toolbox-python would provide functions to construct \( \mathbf{H}(\mathbf{q}) \).

import numpy as np

# Example dimensions: floating base (6) + n joints
n_joints = 12
ndof = 6 + n_joints

# Configuration vector q = [q_B; q_J]
q = np.zeros(ndof)

# Generalized velocity v = [V_B; qdot_J]
v = np.zeros(ndof)

# Example: set some nonzero base twist and joint velocities
# V_B = [omega_x, omega_y, omega_z, v_x, v_y, v_z]
v[0:6] = np.array([0.0, 0.0, 0.5, 0.1, 0.0, 0.0])   # base twist
v[6:]  = 0.2 * np.ones(n_joints)                   # joint speeds

# Placeholder inertia matrix H(q); in practice use a library call:
#   H = pinocchio.crba(model, data, q)
H = np.eye(ndof)  # for demonstration, identity inertia

# Generalized momentum p = H(q) v
p = H.dot(v)

print("Generalized velocity v:", v)
print("Generalized momentum p:", p)
      

With Pinocchio (Python bindings), the essential pattern is:

import pinocchio as pin

# model, data constructed from a URDF including a floating base
# by specifying a joint model like JointModelFreeFlyer
# model, data = pin.buildModelFromUrdf(urdf_path, pin.JointModelFreeFlyer())

q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)

# Composite rigid-body algorithm to compute H(q)
H = pin.crba(model, data, q)

p = H.dot(v)
      

Here model.nv equals \( n+6 \). The first six entries correspond to the floating base twist degrees of freedom; this convention matches the formulation used in this lesson.

In C++, rigid-body dynamics libraries such as RBDL or Pinocchio use Eigen data structures for vectors and matrices. The core generalized momentum operation is still \( \mathbf{p} = \mathbf{H}(\mathbf{q})\mathbf{v} \).

#include <iostream>
#include <Eigen/Dense>

int main() {
  const int n_joints = 12;
  const int ndof = 6 + n_joints;

  Eigen::VectorXd q(ndof);
  Eigen::VectorXd v(ndof);
  Eigen::MatrixXd H(ndof, ndof);

  q.setZero();
  v.setZero();
  H.setIdentity(); // Example only; real H(q) from dynamics library

  // Example base twist
  v.segment<6>(0) << 0.0, 0.0, 0.5, 0.1, 0.0, 0.0;
  // Example joint velocities
  v.segment(6, n_joints).setConstant(0.2);

  // Generalized momentum
  Eigen::VectorXd p = H * v;

  std::cout << "v = " << v.transpose() << std::endl;
  std::cout << "p = " << p.transpose() << std::endl;

  return 0;
}
      

With RBDL (Rigid Body Dynamics Library), one would typically construct an rbdl::Model including a floating base joint (e.g. a 6D joint), and then call the Composite Rigid Body Algorithm:

#include <rbdl/rbdl.h>
// ...
RigidBodyDynamics::Model model;
RigidBodyDynamics::Math::VectorNd q = RigidBodyDynamics::Math::VectorNd::Zero(model.dof_count);
RigidBodyDynamics::Math::MatrixNd H = RigidBodyDynamics::Math::MatrixNd::Zero(model.dof_count, model.dof_count);

// Build model with a floating base joint (6D) and n joints
// ...

RigidBodyDynamics::CompositeRigidBodyAlgorithm(model, q, H, /*update_kinematics=*/true);
RigidBodyDynamics::Math::VectorNd v = RigidBodyDynamics::Math::VectorNd::Zero(model.dof_count);
// Fill v with base twist and joint velocities
RigidBodyDynamics::Math::VectorNd p = H * v;
      

Again, the first six velocity coordinates in v correspond to the floating base, matching the generalized coordinates used to build the model.

Java does not have a dominant general-purpose robotics dynamics library, but one can use matrix libraries such as EJML or Apache Commons Math to implement generalized momentum computations. Below is an EJML-based sketch.

import org.ejml.simple.SimpleMatrix;

public class FloatingBaseMomentum {
    public static void main(String[] args) {
        int nJoints = 12;
        int ndof = 6 + nJoints;

        // Generalized coordinates q and velocities v
        SimpleMatrix q = new SimpleMatrix(ndof, 1);
        SimpleMatrix v = new SimpleMatrix(ndof, 1);

        // Example inertia matrix H(q); identity for demonstration
        SimpleMatrix H = SimpleMatrix.identity(ndof);

        // Set example base twist (omega_x, omega_y, omega_z, v_x, v_y, v_z)
        v.set(0, 0.0);
        v.set(1, 0.0);
        v.set(2, 0.5);
        v.set(3, 0.1);
        v.set(4, 0.0);
        v.set(5, 0.0);

        // Set joint velocities
        for (int i = 6; i < ndof; ++i) {
            v.set(i, 0.2);
        }

        // Generalized momentum p = H(q) v
        SimpleMatrix p = H.mult(v);

        System.out.println("v = ");
        v.print();
        System.out.println("p = ");
        p.print();
    }
}
      

In a full implementation, one would:

1. Parse a robot description (e.g. URDF or custom format).
2. Build link inertias and joint transforms.
3. Implement a composite rigid-body algorithm in Java using EJML.
4. Expose methods massMatrix(q) and generalizedMomentum(q, v).

The core algebraic structure remains identical to the formulations in this lesson.

MATLAB's Robotics System Toolbox provides a rigidBodyTree class that can represent floating-base robots by specifying an appropriate base joint type and link structure. The mass matrix and generalized momentum can then be computed symbolically or numerically.

% Define a rigidBodyTree with a floating base
robot = rigidBodyTree("DataFormat","row","MaxNumBodies",20);

% Example: free-floating base as 6-DoF joint (in practice, use addBody etc.)
% Here we assume 'robot' has already been built from a URDF with a floating base.

n = robot.NumBodies;         % number of rigid bodies (not DoFs)
ndof = robot.NumBodies + 6;  % generalized DoFs approx.

% Random configuration and velocities
q = homeConfiguration(robot);
q = [zeros(1,6), [q.JointPosition]];   % prepend floating base coords
v = zeros(1, ndof); v(1,1:6) = [0,0,0.5,0.1,0,0];

% Mass matrix H(q)
H = massMatrix(robot, q);

% Generalized momentum p = H v'
p = (H * v.').';

disp("Generalized momentum p:");
disp(p);
      

In Simulink, one can construct a block diagram with:

• A block that outputs \( \mathbf{q} \) and \( \mathbf{v} \).
• A custom MATLAB Function block that computes \( \mathbf{H}(\mathbf{q}) \) and \( \mathbf{p} = \mathbf{H}(\mathbf{q})\mathbf{v} \).
• Visualization blocks to monitor the evolution of generalized momentum during simulation.

This connects the abstract generalized momentum formulation with time-domain simulation pipelines for floating-base robots.

Mathematica is well-suited for symbolic derivation of generalized inertia matrices and momenta. Below we show a simple symbolic planar floating-body example similar to Section 6.

(* Define symbolic variables *)
Clear[x, y, theta, xdot, ydot, thetadot, m, I];

q = {x, y, theta};
qdot = {xdot, ydot, thetadot};

(* Kinetic energy for planar floating rigid body *)
T = 1/2 m (xdot^2 + ydot^2) + 1/2 I thetadot^2;

(* Inertia matrix H(q) from quadratic form *)
H = Simplify[Grad[Grad[T, qdot].qdot, qdot]];

(* Generalized momentum p = dT/d(qdot) = H qdot *)
p = Grad[T, qdot] // Simplify;

H // MatrixForm
p // MatrixForm
      

For 3D multi-body systems, one can:

1. Use rotation matrices (or quaternions) and homogeneous transforms for each link.
2. Symbolically compute each link's spatial velocity as a function of \( \mathbf{q} \) and \( \mathbf{v} \).
3. Form the kinetic energy as a sum over links.
4. Extract \( \mathbf{H}(\mathbf{q}) \) and \( \mathbf{p} \) via symbolic differentiation.

Although computationally heavy, this approach is valuable for verifying numerical algorithms or deriving simplified analytic models for low-DoF systems.

In this lesson we generalized fixed-base multi-body dynamics to floating-base robots by introducing a configuration manifold \( \mathcal{Q} = SE(3) \times \mathbb{R}^n \) and corresponding generalized velocities combining base twist and joint velocities. We defined the generalized inertia matrix \( \mathbf{H}(\mathbf{q}) \) and showed that generalized momentum \( \mathbf{p} = \mathbf{H}(\mathbf{q})\mathbf{v} \) arises naturally as the canonical momentum of the Lagrangian. The block structure of \( \mathbf{H} \) separates base and joint momenta and makes transparent the coupling between base motion and internal motion. We also showed how to compute generalized momentum numerically in Python, C++, Java, MATLAB/Simulink, and Mathematica, paving the way for the whole-body kinematics and centroidal dynamics topics in the rest of this chapter.