Complex robots such as humanoids or quadrupeds are naturally modeled as floating-base kinematic trees: a free-floating base link (with six DoFs in SE(3)) to which multiple kinematic chains (arms, legs, head, torso branches) are attached. Unlike classical industrial manipulators, the base is not fixed in the world, so the whole-body motion couples base motion and joint motion.

Let the robot have a floating base and \( n \) actuated joints. We collect generalized coordinates as

\[ \mathbf{q}_\text{fb} \in \mathrm{SE(3)}, \quad \mathbf{q} \in \mathbb{R}^n,\quad \mathbf{v}_\text{fb} \in \mathbb{R}^6,\quad \dot{\mathbf{q}} \in \mathbb{R}^n. \]

Here \( \mathbf{q}_\text{fb} \) is the base pose, and \( \mathbf{v}_\text{fb} \) is the base spatial twist (6D velocity). For each link \( i \), we define a body-fixed frame and denote the homogeneous transform from the world frame 0 to link \( i \) as \( \,^{0}\mathbf{T}_i(\mathbf{q}_\text{fb},\mathbf{q}) \in \mathrm{SE(3)} \).

The key modeling principle in a tree is that each link has exactly one parent (except the floating base), and the transform of a link is obtained by composing its parent transform with its own joint transform. This leads to efficient recursive algorithms for forward kinematics and for computing whole-body Jacobians.

flowchart TD
  B["Floating base (root link)"] --> T["Torso link"]
  T --> LA["Left arm chain"]
  T --> RA["Right arm chain"]
  T --> LH["Left hip"]
  T --> RH["Right hip"]
  LH --> LL["Left leg chain"]
  RH --> RL["Right leg chain"]
        

Each node in the tree is a link, each edge is a joint, and kinematics propagates from the root to the leaves. This unified view will be reused later for dynamics and contact modeling.

A whole-body kinematic structure can be represented as a rooted tree \( \mathcal{T} = (\mathcal{V}, \mathcal{E}) \), where

• \( \mathcal{V} = \{0,1,\dots,N\} \) is the set of links (nodes).
• \( 0 \) is the floating base index, used as the root of the tree.
• \( \mathcal{E} \subset \mathcal{V} \times \mathcal{V} \) is the set of directed edges \( (p(i), i) \) from parent link \( p(i) \) to child link \( i \).

We assume that each non-root link has a unique parent, so we can encode the topology by a parent map

\[ \pi : \{1,\dots,N\} \to \{0,\dots,N\}, \quad \pi(i) = p(i), \]

and define the set of children of a link as \( \mathcal{C}(i) = \{ j \mid \pi(j) = i \} \). The tree property implies that for any link \( i \) there exists a unique path from the base to that link:

\[ 0 = i_0 \to i_1 \to \dots \to i_{k-1} \to i_k = i. \]

For each joint/link pair \( i \) we associate:

• A joint coordinate \( q_i \) (or vector if multi-DoF); these collect into \( \mathbf{q} \in \mathbb{R}^n \).
• A joint configuration-dependent transform \( \,^{\pi(i)}\mathbf{T}_i(q_i) \in \mathrm{SE(3)} \), expressed in the parent frame.
• A motion subspace (joint screw) \( \mathbf{S}_i \in \mathbb{R}^{6 \times 1} \) for 1-DoF joints, representing the twist produced by unit joint velocity.

If we denote the floating base transform by \( \,^{0}\mathbf{T}_\text{fb}(\mathbf{q}_\text{fb}) \), then the world transform of any link \( i \) is the ordered product along the unique path from the base to \( i \):

\[ \,^{0}\mathbf{T}_i(\mathbf{q}_\text{fb},\mathbf{q}) = \,^{0}\mathbf{T}_\text{fb}(\mathbf{q}_\text{fb}) \prod_{\ell=1}^k \,^{i_{\ell-1}}\mathbf{T}_{i_\ell}(q_{i_\ell}). \]

Because the tree has no cycles, this product is well defined and unique. This property underlies all recursive algorithms used in robotics libraries such as URDF-based toolboxes, Pinocchio, and RBDL.

In earlier chapters we derived forward kinematics for open chains. For a general tree, we reuse the same idea but traverse links in a topological order consistent with the parent map \( \pi \). Let \( \mathbf{X}_i^{0} \) denote the spatial motion transform from link \( i \) coordinates to world coordinates (6×6 matrix) and \( \mathbf{v}_i \in \mathbb{R}^6 \) the spatial velocity of link \( i \). For each link we have the standard recursive relations

\[ \,^{0}\mathbf{T}_i = \,^{0}\mathbf{T}_{\pi(i)}\,^{\pi(i)}\mathbf{T}_i(q_i), \]

\[ \mathbf{v}_i = \mathbf{X}_i^{\pi(i)} \mathbf{v}_{\pi(i)} + \mathbf{S}_i \dot{q}_i, \]

where \( \mathbf{X}_i^{\pi(i)} \) is the motion transform corresponding to \( \,^{\pi(i)}\mathbf{T}_i \). For the floating base we set

\[ \,^{0}\mathbf{T}_\text{fb}(\mathbf{q}_\text{fb}) = \mathbf{g}_0, \quad \mathbf{v}_\text{fb} = \mathbf{v}_0, \]

where \( \mathbf{g}_0 \) and \( \mathbf{v}_0 \) are the base pose and base twist in world coordinates.

A generic algorithm to compute all link transforms and spatial velocities is:

flowchart TD
  S["Start: base pose and base twist"] --> O["Compute topological order of links"]
  O --> L1["For each link i in order"]
  L1 --> JTF["Compute joint transform T_parent_to_i(q_i)"]
  JTF --> TF["Update world transform T0_i = T0_parent * T_parent_to_i"]
  TF --> SV["Update spatial velocity v_i from parent and joint"]
  SV --> L1
        

This recursion is linear in the number of links and naturally handles complex branching structures, as long as the parent map is acyclic and rooted at the floating base.

We now derive whole-body Jacobians that map generalized velocities to link spatial twists. Define the generalized velocity vector as

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

For each link \( i \) we define a whole-body Jacobian \( \mathbf{J}_i(\mathbf{q}_\text{fb},\mathbf{q}) \in \mathbb{R}^{6\times(6+n)} \) such that

\[ \mathbf{v}_i = \mathbf{J}_i(\mathbf{q}_\text{fb},\mathbf{q}) \,\mathbf{v}. \]

Partition \( \mathbf{J}_i \) into base and joint parts:

\[ \mathbf{J}_i = \big[\, \mathbf{J}_i^\text{base} \;\; \mathbf{J}_i^\text{jnt} \,\big], \quad \mathbf{J}_i^\text{base} \in \mathbb{R}^{6\times 6},\; \mathbf{J}_i^\text{jnt} \in \mathbb{R}^{6\times n}. \]

The base part is simply the motion transform from the base frame to the link frame:

\[ \mathbf{J}_i^\text{base} = \mathbf{X}_i^{0}(\mathbf{q}_\text{fb},\mathbf{q}), \]

because a base twist \( \mathbf{v}_0 \) induces a link twist \( \mathbf{v}_i = \mathbf{X}_i^{0} \mathbf{v}_0 \) when all joints are locked.

For the joint part, recall that only joints on the unique path from the base to link \( i \) influence \( \mathbf{v}_i \). Let \( \operatorname{Anc}(i) \) denote the set of ancestor joints of link \( i \). Then the column corresponding to joint \( j \) is

\[ \mathbf{J}_i^{\text{jnt},j} = \begin{cases} \mathbf{X}_i^{j}(\mathbf{q})\,\mathbf{S}_j, & \text{if } j \in \operatorname{Anc}(i),\\ \mathbf{0}, & \text{otherwise}. \end{cases} \]

Here \( \mathbf{X}_i^{j} \) is the motion transform from joint \( j \) coordinates to link \( i \) coordinates. Using the recursive velocity relation from Section 3, one can prove by induction on the tree depth that

\[ \mathbf{v}_i = \mathbf{X}_i^{0} \mathbf{v}_0 + \sum_{j \in \operatorname{Anc}(i)} \mathbf{X}_i^{j} \mathbf{S}_j \dot{q}_j, \]

which is exactly the Jacobian representation. Stacking multiple link Jacobians vertically yields whole-body task Jacobians for multiple end-effectors or internal points.

Efficient implementation requires a compact representation of the kinematic tree. A typical structure (used in URDF-parsing libraries, Pinocchio, RBDL, etc.) is:

• Parent index array: \( \pi(i) \) for \( i=1,\dots,N \).
• Joint type and motion subspace: revolute/prismatic/fixed and \( \mathbf{S}_i \in \mathbb{R}^{6\times1} \).
• Home transform: the constant offset \( \,^{\pi(i)}\mathbf{T}_i(0) \) when \( q_i = 0 \).
• Kinematic parameters: joint axes, origin positions, etc.

During computation, one additionally stores:

• World transforms \( \,^{0}\mathbf{T}_i \) for all links.
• Spatial velocities \( \mathbf{v}_i \) for all links.
• Optional: cached motion transforms \( \mathbf{X}_i^{\pi(i)} \).

Arrays indexed by link index together with the parent map make iterations cache-friendly and amenable to low-level optimization.

Below is a simplified Python implementation of a floating-base kinematic tree using numpy. We represent transforms as \(4\times4\) homogeneous matrices and spatial motion transforms as \(6\times6\) matrices. This code illustrates the recursion from Sections 3 and 4. In practice, a production system would use a specialized library such as pinocchio or pybullet.

import numpy as np

def skew(v):
    """Skew-symmetric matrix [v]x for v in R^3."""
    return np.array([[0.0, -v[2], v[1]],
                     [v[2],  0.0, -v[0]],
                     [-v[1], v[0], 0.0]])

def exp_se3(xi, theta):
    """
    Exponential map for a screw axis xi in R^6 and scalar theta.
    xi = [omega; v]. Uses standard SE(3) matrix exponential.
    """
    omega = xi[0:3]
    v = xi[3:6]
    w_norm = np.linalg.norm(omega)
    if w_norm < 1e-9:
        R = np.eye(3)
        p = v * theta
    else:
        wn = omega / w_norm
        wx = skew(wn)
        R = (np.eye(3)
             + np.sin(w_norm * theta) * wx
             + (1.0 - np.cos(w_norm * theta)) * (wx @ wx))
        V = (np.eye(3) * theta
             + (1.0 - np.cos(w_norm * theta)) / w_norm * wx
             + (w_norm * theta - np.sin(w_norm * theta)) / (w_norm ** 2) * (wx @ wx))
        p = V @ v
    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = p
    return T

def adjoint(T):
    """Spatial adjoint transform (6x6) of homogeneous transform T."""
    R = T[0:3, 0:3]
    p = T[0:3, 3]
    Ad = np.zeros((6, 6))
    Ad[0:3, 0:3] = R
    Ad[3:6, 3:6] = R
    Ad[3:6, 0:3] = skew(p) @ R
    return Ad

class Link:
    def __init__(self, parent, S, T_parent_to_i0):
        """
        parent: integer parent index
        S: 6x1 motion subspace vector of the joint
        T_parent_to_i0: 4x4 transform at q_i = 0
        """
        self.parent = parent
        self.S = S.reshape(6, 1)
        self.T_parent_to_i0 = T_parent_to_i0

class KinematicTree:
    def __init__(self, links):
        """
        links: list of Link objects, index 0 reserved for floating base pseudo-link.
        """
        self.links = links
        self.N = len(links) - 1  # number of actuated joints/links except base

    def forward_kinematics(self, q_base, v_base, q):
        """
        q_base: 4x4 homogeneous transform of floating base in world
        v_base: 6-dim spatial twist of floating base in world
        q: array of joint positions (size N)
        Returns:
            T_world: list of 4x4 transforms for each link (0..N)
            v_world: list of 6x1 twists for each link (0..N)
        """
        T_world = [None] * len(self.links)
        v_world = [None] * len(self.links)

        # base
        T_world[0] = q_base
        v_world[0] = v_base.reshape(6, 1)

        for i in range(1, len(self.links)):
            link = self.links[i]
            parent = link.parent
            # joint transform from parent to link
            T_joint = exp_se3(link.S.flatten(), q[i-1])
            T_world[i] = T_world[parent] @ link.T_parent_to_i0 @ T_joint

            # spatial velocity recursion
            X_i_parent = adjoint(link.T_parent_to_i0 @ T_joint)
            v_world[i] = X_i_parent @ v_world[parent] + link.S * 0.0
            # The term link.S * 0.0 is a placeholder for adding S * qdot[i-1]
            # once joint velocities are provided.

        return T_world, v_world

    def link_jacobian(self, q_base, q, link_index):
        """
        Compute whole-body Jacobian for a given link index.
        For simplicity, ignore base orientation here and treat J_base as identity.
        """
        # First compute all world transforms
        T_world, _ = self.forward_kinematics(q_base, np.zeros(6), q)

        # Base part of Jacobian: identity (6x6) in this simplified example
        J_base = np.eye(6)

        # Joint part
        n = self.N
        J_jnt = np.zeros((6, n))

        # Walk from link to base collecting ancestor joints
        i = link_index
        visited = []
        while i != 0:
            visited.append(i)
            i = self.links[i].parent
        visited = visited[::-1]  # from base to link

        for idx, i in enumerate(visited):
            link = self.links[i]
            # column index in q-vector
            j = i - 1
            T_i = T_world[link_index]
            T_j = T_world[i]
            T_i_j = np.linalg.inv(T_j) @ T_i
            X_i_j = adjoint(T_i_j)
            J_jnt[:, j] = (X_i_j @ link.S).flatten()

        J = np.concatenate([J_base, J_jnt], axis=1)
        return J
      

A more realistic implementation would distinguish between the fixed base frame of the floating base, the world frame, and handle base orientation consistently, but the structural pattern of parent indexing, recursion, and ancestor-based Jacobian columns is identical.

Using pinocchio, the same ideas are exposed at a high level:

import pinocchio as pin

# Load model from URDF
model = pin.buildSampleModelHumanoidRandom()
data = model.createData()

# q: configuration vector including floating base (dimension model.nq)
# v: velocity vector including base twist (dimension model.nv)
q = pin.randomConfiguration(model)
v = np.zeros(model.nv)

# Forward kinematics and Jacobians in one call
pin.forwardKinematics(model, data, q, v)
frame_id = model.getFrameId("left_foot")
J6 = pin.computeFrameJacobian(model, data, q, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
      

Here the library internally maintains the same tree structure and uses optimized recursive algorithms for transforms and Jacobians.

In C++, whole-body kinematics trees are often implemented using Eigen matrices for linear algebra. The snippet below sketches a minimal structure and forward kinematics routine. Production code usually relies on libraries such as Pinocchio or RBDL, which add automatic differentiation, dynamics, and URDF integration.

#include <Eigen/Dense>
#include <vector>

using Transform = Eigen::Matrix4d;
using SpatialMatrix = Eigen::Matrix<double,6,6>;
using SpatialVector = Eigen::Matrix<double,6,1>;

struct Link
{
  int parent;                 // index of parent link
  SpatialVector S;            // motion subspace
  Transform T_parent_to_i0;   // transform at q_i = 0
};

class KinematicTree
{
public:
  explicit KinematicTree(const std::vector<Link>& links)
    : links_(links)
  {}

  void forwardKinematics(const Transform& T_base,
                         const SpatialVector& v_base,
                         const Eigen::VectorXd& q,
                         std::vector<Transform>& T_world,
                         std::vector<SpatialVector>& v_world) const
  {
    const int n_links = static_cast<int>(links_.size());
    T_world.resize(n_links);
    v_world.resize(n_links);

    // base
    T_world[0] = T_base;
    v_world[0] = v_base;

    for (int i = 1; i < n_links; ++i)
    {
      const Link& link = links_[i];
      int parent = link.parent;

      Transform T_joint = expSE3(link.S, q(i-1)); // user-defined exponential
      T_world[i] = T_world[parent] * link.T_parent_to_i0 * T_joint;

      SpatialMatrix X_i_parent = adjoint(link.T_parent_to_i0 * T_joint);
      v_world[i] = X_i_parent * v_world[parent]; // + link.S * qdot(i-1) when needed
    }
  }

private:
  std::vector<Link> links_;
};
      

A whole-body Jacobian method would follow the pattern described in the Python code: traverse ancestors of the query link, compute relative motion transforms, and fill the corresponding columns.

Pinocchio exposes similar operations through its C++ API:

#include <pinocchio/algorithm/jacobian.hpp>

pinocchio::Model model;
pinocchio::buildModels::humanoidRandom(model);
pinocchio::Data data(model);

Eigen::VectorXd q = pinocchio::randomConfiguration(model);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);

// forward kinematics
pinocchio::forwardKinematics(model, data, q, v);

// Jacobian of a given frame
pinocchio::FrameIndex fid = model.getFrameId("left_foot");
Eigen::Matrix<double,6,Eigen::Dynamic> J6;
pinocchio::computeFrameJacobian(model, data, q, fid,
                                pinocchio::LOCAL_WORLD_ALIGNED, J6);
      

Java does not have a de-facto standard robotics library, but linear algebra packages such as EJML can be used to implement whole-body kinematics trees. Below is a minimal sketch:

import org.ejml.simple.SimpleMatrix;
import java.util.ArrayList;
import java.util.List;

class Link {
    int parent;
    SimpleMatrix S;              // 6x1
    SimpleMatrix T_parent_to_i0; // 4x4

    Link(int parent, SimpleMatrix S, SimpleMatrix T_parent_to_i0) {
        this.parent = parent;
        this.S = S;
        this.T_parent_to_i0 = T_parent_to_i0;
    }
}

class KinematicTree {
    List<Link> links;

    KinematicTree(List<Link> links) {
        this.links = links;
    }

    void forwardKinematics(SimpleMatrix T_base,
                           SimpleMatrix v_base,
                           double[] q,
                           List<SimpleMatrix> T_world,
                           List<SimpleMatrix> v_world) {
        int n = links.size();
        T_world.clear();
        v_world.clear();
        for (int i = 0; i < n; ++i) {
            T_world.add(SimpleMatrix.identity(4));
            v_world.add(new SimpleMatrix(6, 1));
        }

        T_world.set(0, T_base);
        v_world.set(0, v_base);

        for (int i = 1; i < n; ++i) {
            Link link = links.get(i);
            int parent = link.parent;

            SimpleMatrix T_joint = expSE3(link.S, q[i - 1]); // user-defined
            SimpleMatrix T_pi = T_world.get(parent)
                    .mult(link.T_parent_to_i0)
                    .mult(T_joint);
            T_world.set(i, T_pi);

            SimpleMatrix X_i_parent = adjoint(link.T_parent_to_i0.mult(T_joint));
            SimpleMatrix v_i = X_i_parent.mult(v_world.get(parent));
            v_world.set(i, v_i);
        }
    }
}
      

The same structure supports whole-body Jacobian computation by traversing the ancestor chain and accumulating columns, mirroring the derivation in Section 4.

MATLAB's Robotics System Toolbox provides rigidBodyTree, a ready-made representation of kinematic trees (including floating-base support via base joints). The following code constructs a simple legged robot and computes whole-body transforms and Jacobians:

% Define a simple floating-base tree with one leg
robot = rigidBodyTree("DataFormat","column","MaxNumBodies",5);

% Floating base modeled as 6-DoF joint
base = rigidBody("base");
jb = rigidBodyJoint("base6dof","6dof");
setFixedTransform(jb, eye(4));
base.Joint = jb;
addBody(robot, base, robot.BaseName);

% Add a hip, knee, ankle chain
hip = rigidBody("hip");
j_hip = rigidBodyJoint("j_hip","revolute");
setFixedTransform(j_hip, trvec2tform([0 0 0.5]));
j_hip.JointAxis = [0 1 0];
hip.Joint = j_hip;
addBody(robot, hip, "base");

knee = rigidBody("knee");
j_knee = rigidBodyJoint("j_knee","revolute");
setFixedTransform(j_knee, trvec2tform([0 0 -0.4]));
j_knee.JointAxis = [0 1 0];
knee.Joint = j_knee;
addBody(robot, knee, "hip");

ankle = rigidBody("ankle");
j_ankle = rigidBodyJoint("j_ankle","revolute");
setFixedTransform(j_ankle, trvec2tform([0 0 -0.4]));
j_ankle.JointAxis = [1 0 0];
ankle.Joint = j_ankle;
addBody(robot, ankle, "knee");

% Configuration vector includes 6 base DoFs + 3 leg joints
q = homeConfiguration(robot);
q(1:6) = [0; 0; 0.9; 0; 0; 0]; % base position/orientation (XYZRPY)

% Forward kinematics: transform of ankle
T0ankle = getTransform(robot, q, "ankle");

% Whole-body geometric Jacobian for ankle
[J_ankle, ~] = geometricJacobian(robot, q, "ankle");
      

In Simulink, the same rigidBodyTree can be used via dedicated blocks (e.g., for forward kinematics or Jacobian computation), integrating seamlessly with control and estimation subsystems.

Mathematica is convenient for symbolic derivation of transforms and Jacobians in tree-structured robots. Below is an example with a floating base and a simple 2-link arm attached to the torso:

(* Define symbolic variables *)
ClearAll[qb, q1, q2];
qb = {xb, yb, zb, rollb, pitchb, yawb};
q1 = Symbol["q1"];
q2 = Symbol["q2"];

rotZ[theta_] := { {Cos[theta], -Sin[theta], 0},
                 {Sin[theta],  Cos[theta], 0},
                 {0,           0,          1} };

trans[p_] := { {1, 0, 0, p[[1]]},
              {0, 1, 0, p[[2]]},
              {0, 0, 1, p[[3]]},
              {0, 0, 0, 1} };

hom[R_, p_] := ArrayFlatten[{ {R, Transpose[{p}]},
                             { {0, 0, 0}, {1} } }];

(* Floating base pose as composition of translations and rotations *)
T0b = trans[{xb, yb, zb}].hom[rotZ[yawb], {0, 0, 0}];

(* Torso offset from base *)
TbTorso = trans[{0, 0, 0.5}];

(* Joint transforms for arm *)
S1 = {0, 0, 1, 0, 0, 0}; (* revolute about z *)
S2 = {0, 1, 0, 0, 0, 0}; (* revolute about y *)

T_torso_1[q_] := hom[rotZ[q], {0, 0, 0.3}];
T_1_2[q_] := hom[rotZ[q], {0, 0, 0.3}];

(* World transform of link 2 (end-effector) *)
T0_2[qbase_List, q1_, q2_] :=
  T0b /. Thread[{xb, yb, zb, yawb} -> qbase] .
  TbTorso .
  T_torso_1[q1] .
  T_1_2[q2];

(* Position of end-effector *)
p0 = T0_2[qb, q1, q2][[1 ;; 3, 4]];

(* Whole-body Jacobian w.r.t. [q1, q2] *)
Jq = D[p0, {{q1, q2}}];
      

By extending this pattern to all branches and including the base parameters as generalized coordinates, one can symbolically derive full whole-body Jacobians and export them as optimized numerical functions.

In this lesson we modeled floating-base robots as rooted kinematic trees, formalizing parent maps, path uniqueness, and the composition of link transforms from base and joint transforms. Using spatial vector notation, we derived recursive formulas for link transforms and velocities and established the whole-body Jacobian structure, where each column corresponds to an ancestor joint or the floating base.

We then showed how these concepts are implemented in common programming environments (Python, C++, Java, MATLAB/Simulink, and Mathematica) via compact parent-indexed data structures and recursive algorithms. These whole-body kinematics trees form the backbone for centroidal dynamics, contact modeling, and whole-body motion in subsequent lessons.