🤖 Mastering Rigid Body Systems Modeling

When exploring legged robots and manipulator controls, you’ll often find that tools for modeling robot dynamics, such as Pinocchio, Drake, and MuJoCo, are frequently used. I noticed that Featherstone’s notation for rigid body dynamics algorithms is often employed behind the scenes. Although rarely covered in detail in robotics courses, I found this topic intriguing and wanted to explore it further. This blog post is the first in a series exploring the internals of these libraries and rigid body dynamics algorithms in general.

Rigid-body systems Dynamics (RBD) is a small yet foundational area in robotics and physics-based animation. Its importance lies in providing algorithms to model robots as multibody dynamical systems and calculate essential quantities for planning, controlling robot motion, and simulation. The theory behind these algorithms dates back to the 18th century but has been recently revisited thanks to the foundational work of Roy Featherstone, whose name appears frequently in this field.

Rigid-body systems Dynamics encompasses a wide range of topics, including:

• Recursive Newton-Euler Algorithm (RNEA)
• Composite Rigid Body Algorithm (CRBA)
• Articulated Body Algorithm (ABA)
• Hybrid and Contact Dynamics
• Dynamics Derivatives
• And more…

This article focuses on the fundamentals: Rigid Body Systems Modeling. Building a solid foundation in this area will simplify understanding more complex algorithms. Even without going deeply into these algorithms, developing a good intuition for robot modeling is highly beneficial for any robotics enthusiast. This understanding helps you become comfortable with popular modeling specifications in the industry, such as URDF, Mujoco XML, or SDF and improves spatial reasoning.

Motivation for Rigid Body Systems Modeling

Consider the task of controlling a robotic manipulator in the Task Space.

To achieve this, you need to implement the following:

1. Compute the current position and orientation of the end-effector to calculate errors that can be transformed into control signals.
2. Compute the Jacobian to map joint velocities to operational space.
3. Compute the mass matrix, which is used to calculate inverse dynamics.

As Featherstone stated in his book, there are essentially two approaches:

1. Derive the symbolic equations of motion for the given system and translate these equations into computer code.
2. Construct a system model describing the given system and supply it as an argument to a model-based dynamics calculation routine.

This observation is crucial because it highlights how a specialized model describing the robotic system provides a higher level of abstraction in software. Instead of deriving and using equations of motion for each specific robot, a robot-agnostic model enables the creation of general-purpose algorithms to calculate required dynamic quantities such as the mass matrix or the vector of non-linear effects.

This approach has parallels with generic programming in computer science. Just as stdlib in C++ or streams in Java are agnostic to the containers they operate on, rigid body dynamics algorithms are agnostic to specific robot models.

To illustrate the idea, here is the code:

Operational space controller code

VectorX compute_control_torques(MultibodyModel const &model, GainMatrices const &gains)
{
  auto q = get_joint_positions();
  auto qd = get_joint_velocities();

  auto [wPee, wRee] = compute_end_effector_placement(model, q);

  auto wPd = get_desired_position();
  auto wRd = get_desired_orientation();
  auto eeRd = wRee.transpose() * wRd;
  auto [delta_theta, r_hat] = get_angle_axis_from_SO3(eeRd);

  auto eeO_error = delta_theta * r_hat;// expressed in the end-effector frame
  auto wO_error = wRee * eeO_error;// expressed in the world frame

  auto J = compute_end_effector_frame_jacobian(model, q, ReferenceFrame::LOCAL_WORLD_ALIGNED);

  auto twist = J * qd;
  auto v = twist.tail<3>();
  auto omega = twist.head<3>();

  auto [Kxp, Kxd, Kop, Kod] = gains;

  SpatialVector F;
  F.tail(3) = Kxp * (wPd - wPee) + Kxd * (-v);
  F.head(3) = Kop * wO_error + Kod * (-omega);

  auto [M, C] = compute_mass_matrix_and_nle(model, SystemConfiguration{ q, qd });
  auto lambda = pinv(J * M.inverse() * J.transpose());

  return J.transpose() * lambda * F + C;
}

Take a look at the code highlighted with purple rectangles.

compute_end_effector_placement, compute_end_effector_frame_jacobian, and compute_mass_matrix_and_nle are all generic RBD algorithms that accept a robot model as an argument.

While model-based dynamics software is advantageous because each algorithm can be implemented once and work on any rigid-body system supplied, there are also alternatives. One such alternative is code generation, which is popular but not covered in this article. Code generation involves creating custom code designed to work on a single rigid-body system or a narrow class of similar systems. This approach can exploit prior knowledge of joint types, geometric and inertia parameter values, and more, resulting in fewer arithmetic operations needed for the calculation.

Rigid Body Systems Modeling: Featherstone’s notation

Let’s begin with a high-level overview. Imagine we want to create a model representation for an arbitrary robotic manipulator.

Since we’re avoiding modeling the manipulator by deriving equations of motion, we need to develop an alternative approach.

By simplifying the complexities of mechanical design (such as pulleys, reducers, and gears), we can conceptualize a robotic manipulator as a set of interconnected rigid bodies. Two links are distinctive:

1. One connected to a stand or platform
2. The end effector, designed to interact with the environment

The end effector is typically attached to a gripper or tooling that provides the robot’s functional purpose.

Intuitively, the model should describe:

• Each individual link in the system
• The connections between links
• The spatial positioning of links
• Their relative movements

For now, we’ll focus on the connectivity properties of the system, setting aside the properties of individual links.

Connectivity

To describe an arbitrary manipulator, it’s advantageous to use a generalizable medium for representing connections between components. What better tool exists for describing connections — from internet pages to London Tube stations — than a graph?

Intuitively, using links as nodes in this graph seems logical. In such an abstract representation, we only need a unique identifier for each link, which can be achieved through simple enumeration.

How should these nodes be connected? Naturally, we connect nodes with an edge if their corresponding links are physically connected.

Below is an example of the connectivity graph for the IIWA 14 manipulator. As illustrated, the structure is typically simple for most industrial manipulators, essentially forming a degenerate tree or linked list.

The connectivity graph for the IIWA14 manipulator

However, this isn’t universal. More complex robots or mechanisms may have graphs representing branched trees or even containing cycles.

As we develop this conceptual representation of link connectivity, we can consider its software implementation. While those with computer science backgrounds might think of adjacency lists or matrices, these aren’t optimal for multibody modeling. Let’s identify task properties that could guide our choice of representation.

Lowering our abstraction level, recall that a robot is a physical system where graph connections have physical meaning. Nodes represent robot links, and edges typically represent joints. Physically, a joint is a complex mechanical unit connecting robot parts, usually transferring actuator forces between bodies. Commonly, link A contains a mechanism with a motor and gears applying torque to child link B, transforming the joint’s control signal into the motion of link B relative to A.

Given this, it’s natural for our software representation to distinguish parent and child links for each graph connection.

For simple systems like robotic arms, representing the connectivity graph and parent-child relationships is sufficient. We can store the number of links and each link’s parent index efficiently using a simple array.

In code, it looks straightforward:

struct MultibodyModel
{
  int n_bodies{};
  std::vector<int> parent{};
  
  ...
};

For the IIWA manipulator shown above, the values are as follows:

The parent array for the IIWA14 manupulator

The concept is simple, but note an interesting point: the manipulator’s base link isn’t included in the graph. While counterintuitive, the base link doesn’t contribute to other links’ dynamics. It only appears as -1 for direct children in the parent array. Additionally, in our model, the number of bodies equals the number of joints, which might be misleading. In reality, for a simple serial manipulator, the number of links is one more than the number of joints.

This simple connectivity representation suffices for the most popular industrial manipulators — non-branched robots (kinematic chains) without loops. The representation is more complex for branched systems or those with loops; I may cover this in another article.

The next section will discuss modeling link positioning in space, also known as the system’s geometry.

System’s geometry

In Featherstone’s notation, each body is assigned a coordinate frame $F_i$ ​ - the coordinate frame for the i-th body

To accurately describe the system’s motion at each instant in time, you need to know the positions and orientations of each $F_i$ relative to each other.

Our objective for this section is to effectively integrate this information into our MultibodyModel.

In Featherstone’s notation, this information is encoded using an array of transformation matrices, X_tree. Here, X_tree[j] describes the transformation from $F_{parent[i]}$ to $F_i$ when the joint is in its zero position.

The reasoning behind this solution could be as follows:

• Transformation matrices are an excellent way to represent the position and orientation of one frame relative to another. Furthermore, they allow us to change the reference frame of other vectors.
• By having a transformation from $F_{parent[i]}$ to $F_i$ , we can change the reference frame of a some vector in a chain-like manner. For instance, if we have a vector ${}^{F_0}_{}V^{}_{}$ , and given X_tree, we can iteratively re-express it in any link frame like so:

${}^{F_i}_{}V^{}_{} = X_{tree}[i] * {}^{F_{i-1}}_{}V^{}_{}$ .

• The transformation from $F_{parent[i]}$ to $F_i$ is not static; it depends on the corresponding joint position. X_tree describes this transformation only for the zero position of the joints, essentially describing the geometry of our manipulator when it’s in a default position. In the following section, we will discuss how to account for joint movement and calculate the transformation from $F_{parent[i]}$ to $F_i$ ​ for any given joint position.

The question arises - what exactly is the transformation in this context? Featherston’s notation employs the concept of spatial transformation. This idea is rooted in Spatial Algebra. However, given the complexity of the subject, providing a comprehensive overview within a short article may not be feasible. Nonetheless, I will attempt to offer some insights and intuitive understanding here. For those intrigued, I strongly suggest exploring a few lectures on this topic.

Spatial transformations 101

Spatial transformation matrices are crucial in robotics, particularly for multibody systems’ kinematics and dynamics. For those familiar with robotics concepts like homogeneous matrices and unit quaternions, Featherstone’s notation offers a streamlined approach to understanding spatial relationships between robotic system components. Let’s explore the intuition behind these matrices and how they simplify complex robotic transformations.

Background

In robotics, transformations describe the position and orientation of bodies or components in three-dimensional space. Commonly, 4x4 homogeneous transformation matrices are used, combining rotation and translation to allow straightforward linear algebra for complex spatial movements and rotations.

Featherstone’s Notation

Roy Featherstone’s notation, articulated in his work on spatial algebra, introduces an efficient method for handling interconnected rigid bodies’ dynamics. His approach uses spatial vectors and spatial transformation matrices operating in a six-dimensional space, differing from the familiar 4x4 transformation matrices.

Spatial Vectors

Featherstone uses 6-element spatial vectors combining linear and angular quantities. For example, a spatial velocity vector (twist) consists of angular and linear velocity components. This allows concise representation of motion dynamics and forces/torques in a single framework.

$$V = \begin{bmatrix} \omega \\ v \end{bmatrix}$$ , where $\omega$ is an angular velocity vector, and $v$ is a linear velocity vector.

Spatial Transformation Matrices

The spatial transformation matrix in Featherstone’s notation often denoted as $X$ , transforms 6D spatial vectors between coordinate frames. These 6x6 matrices differ fundamentally from their homogeneous counterparts in structure and interpretation:

1. Structure: A spatial transformation matrix comprises four 3x3 sub-blocks:

   • Upper left: Rotation matrix $R$ (same as in homogeneous matrices)

   • Upper right: Zero matrix

   • Lower left: Zero matrix for pure rotations or a translation vector skew-symmetric matrix times $R$ for general transformations

   • Lower right: Rotation matrix $R$ (repeated)

     $$X = \begin{bmatrix} R & 0 \\ [p]R & R \end{bmatrix}$$

2. Intuition: This structure addresses two aspects of spatial movement:

   • The rotation component affects both angular and linear parts of the spatial vector
   • The translation component affects only the linear part through rotational effects (using a skew-symmetric matrix representing cross products with the translation vector)

Benefits Over Homogeneous Matrices

While homogeneous matrices excel for straightforward kinematic transformations, Featherstone’s spatial transformation matrices offer several advantages:

• Efficiency: Tailored for dynamic calculations where forces and velocities are treated simultaneously
• Clarity: Simplifies equations of motion by clearly separating rotational and translational dynamics
• Unified Representation: Represents both motion (velocities) and force vectors consistently, facilitating Newton-Euler equations application

The Link Between Homogeneous and Spatial Transformation Matrices

The adjoint operator bridges homogeneous and spatial transformation matrices. It expands simple geometrical transformations captured in homogeneous matrices into the six-dimensional domain of spatial matrices.

In code, it looks straightforward:

#include <Eigen/Dense>

using SpatialMatrix = Eigen::Matrix<double, 6, 6>;
using HomogenousMatrix = Eigen::Matrix4d;

SpatialMatrix Ad(const HomogenousMatrix &T)
{
   SpatialMatrix X;
   X.setZero();
   
   // Extract the top-left 3x3 block from T as rotation matrix R
   Eigen::Matrix3d R = T.topLeftCorner<3, 3>();
   
   // Set the top-left and bottom-right 3x3 blocks of X to R
   X.topLeftCorner<3, 3>() = R;
   X.bottomRightCorner<3, 3>() = R;
   
   // Extract the top-right 3x1 block from T as translation vector p
   Eigen::Vector3d p = T.topRightCorner<3, 1>();
   
   // Compute the skew-symmetric matrix of p and multiply by R
   Eigen::Matrix3d p_skew;
   p_skew <<
     0, -p.z(), p.y(),
     p.z(), 0, -p.x(),
     -p.y(), p.x(), 0;
   
   X.bottomLeftCorner<3, 3>() = p_skew * R;
   
   return X;
}

View full source code

With this formalism established, it becomes clear that X_tree is simply an array of spatial transformations, where X_tree[j] represents ${}^{j}X_{parent[j]}$ at a zero joint position.

Let’s update our multibody model code. In addition to the connectivity information (comprising the parent indices array and the number of links), we’ll include an array of spatial transformation matrices. These matrices will represent the required ${}^{j}X_{parent[j]}$ for each link. The code implementation is relatively straightforward:

struct MultibodyModel
{
  int n_bodies{};
  std::vector<int> parent{};
  std::vector<SpatialMatrix> X_tree{};
  
  ...
};

This is a good place to make things more practical. Let’s see how the system’s geometry can be extracted from a real-world URDF file.

URDF 101

Universal Robot Description Format (URDF)

URDF is a widely adopted XML format used in robotics to describe the physical and visual properties of a robot in a structured and readable manner. URDF files play a role in robotics simulation and visualization software, such as ROS (Robot Operating System), enabling accurate and effective modeling of complex robotic systems.

Structure of a URDF File

A URDF file typically describes a robot as a collection of links connected by joints, which forms a kinematic chain or tree structure. Each component (link or joint) is defined with specific XML tags. Here are the primary components:

1. Links: These are the rigid bodies or elements of the robot. Each link is defined by its physical properties (like dimensions and mass), as well as visual (what it looks like) and collision (how it interacts physically with the environment) characteristics.

<link name="link_name">
    <visual>
        ...
    </visual>
    <collision>
       ...
    </collision>
    <inertial>
        ...
    </inertial>
</link>

2. Joints: Joints define the allowable movement between links. Each joint specifies its type (such as revolute, prismatic, fixed, etc.), its connection between parent and child links, and its physical characteristics like axis of rotation and limits.

<joint name="joint_name" type="revolute">
    <parent link="parent_link_name" />
    <child link="child_link_name" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    <axis xyz="0 0 1" />
    <limit lower="-1.57" upper="1.57" effort="100" velocity="0.5" />
</joint>

URDF is essentially another method for modeling a robot. In fact, it’s a textual representation of the connectivity graph, the system’s geometry, and the robot’s links.

Let’s examine a simplified and abbreviated URDF file of the IIWA 14 robot, featuring only two links and their corresponding joint.

<robot name="iiwa14" xmlns:xacro="http://www.ros.org/wiki/xacro">
  ...
  <link name="iiwa_link_1">
    ...
  </link>
  <!-- joint between link_1 and link_2 -->
  <joint name="iiwa_joint_2" type="revolute">
    <parent link="iiwa_link_1"/>
    <child link="iiwa_link_2"/>
    <origin rpy="1.570796326794897   0 3.141592653589793" xyz="0 0 0.2025"/>
    <axis xyz="0 0 1"/>
    ...
  </joint>
  <link name="iiwa_link_2">
    ...
  </link>
  ...
</robot>

How can we extract X_tree from this URDF? All information about geometric properties is located in the corresponding joint elements. In URDF, the joint origin tag describes the position and orientation of the child link relative to the parent link in a human-readable format, which typically includes a displacement vector and roll-pitch-yaw angles or a quaternion. This provides all the information required to calculate the X_tree matrix corresponding to the joint. Given a displacement vector and an orientation quaternion, we can construct a homogeneous matrix that represents the child link frame in the parent link frame, denoted as ${}^{parent[j]}_{}T^{}_{j}$ and it’s straightforward to calculate spatial transformation ${}^{j}_{}X^{}_{parent[j]}$ from ${}^{parent[j]}_{}T^{}_{j}$ using the adjoint operator.

Below is the pseudocode that illustrates this process:

using SE3 = HomogenousMatrix;

{
  ...
    
  SE3 T = Tinv(urdf_pose_to_SE3(joint->parent_to_joint_origin_transform));
  auto X = Ad(T);
  model->X_tree.emplace_back(X);
    
  ...
}

SE3 urdf_pose_to_SE3(::urdf::Pose const &pose /*child frame pose relative to parent*/)
{
  ::urdf::Vector3 const &p = pose.position;
  ::urdf::Rotation const &q = pose.rotation;
  SE3 T;
  T.topLeftCorner<3, 3>() = Quaternion(q.w, q.x, q.y, q.z).matrix();
  T.topRightCorner<3, 1>() = Vector3(p.x, p.y, p.z);
  T(3, 3) = 1.0;

  return T;
}

So far, we have described two key aspects of the system:

1. Connectivity: how links are connected

2. Geometry: how links are positioned in space

In the next section, we will explore how the relative movement of links is modeled using the concept of joints.

Joints

In the previous section, we established the position and orientation of each link in our manipulator when all joints are at their zero positions. This information is encoded in the X_tree variable.

Now let’s allow joints to take arbitrary positions. In this case, the transformation ${}^{j}X_{parent[j]}$ is no longer static and depends on the joint position $\theta$ . It’s also evident that the transformation ${}^{j}X_{parent[j]}$ differs for various types of joints. Consider revolute and prismatic joints: the resulting movement of links is completely different, and this should be reflected in the transformation matrix.

Using these simple observations, Featherstone models ${}^{j}X_{\text{parent}(j)}$ as follows:

${}^{j}{}X^{}_{\text{parent}(j)}$ = $X_{joint}[j](\theta) * X_{tree}[j]$ , where $X_{joint}[j]$ is some transform which depends on the type of joint and the joint posittion $\theta$ ​.

To illustrate the concept, let’s examine the following code fragment:

SpatialMatrix forward_kinematics(MultibodyModel const &model, VectorX const &q) {
  auto const &[n_bodies, parent, X_tree, ...] = model;

  SpatialMatrix jXpj{ SpatialMatrix::Zero() };
  SpatialMatrix wXj{ SpatialMatrix::Identity() };

  for (int j = 0; j < n_bodies; ++j) {
    auto X_joint = ???? // how to store this information in the MultibodyModel?
    jXpj = X_joint * X_tree[j];
    wXj = wXj * jXpj.inverse();
  }

  return wXj;
}

variable jXpj is a ${}^{j}{}X^{}_{\text{parent}(j)}$ and wXj is a ${}^{world}{}X^{}_{j}$

This code illustrates that our model is nearly capable of supporting forward kinematics computations. The qualifier “nearly” is necessary because we still need to determine how we will store information about joint transformations.

In Featherstone’s notation, there is a separate concept of the joint model, which addresses the question: what spatial transformation corresponds to a specific type of joint?

In code, it may look like this:

class JointModel {
public:
    virtual SpatialMatrix joint_transform(double theta) const = 0;
  	...
};

class RevoluteJoint : public JointModel {
public:
    explicit RevoluteJoint(JointAxis axis) : axis(axis) {}

    SpatialMatrix joint_transform(double theta) const override {
        switch (axis) {
        case JointAxis::X:
            return rotx(theta); // spatial rotation matrix around X axis
        case JointAxis::Y:
            return roty(theta); // spatial rotation matrix around Y axis
        case JointAxis::Z: 
            return rotz(theta); // spatial rotation matrix around Z axis
       	...
        }
    }
  
		...
      
private:
    JointAxis axis;
};

class PrismaticJoint : public JointModel {
public:
    explicit PrismaticJoint(Vector3 axis) : axis(axis) {}

    SpatialMatrix joint_transform(double theta) const override {
        return xlt(axis * theta); // spatial translation matrix along axis
    }
  
  	...
    
private:
    Vector3 axis;
};

// More types of joints ...

Using JointModel, we can finalize forward_kinematics as follows:

SpatialMatrix forward_kinematics(MultibodyModel const &model, VectorX const &q) {
 auto const &[n_bodies, parent, X_tree, joint_models, ...] = model;

 SpatialMatrix jXpj{ SpatialMatrix::Zero() };
 SpatialMatrix wXj{ SpatialMatrix::Identity() };

 for (int j = 0; j < n_bodies; ++j) {
   auto X_joint = joint_models[j].joint_transform();
   jXpj = X_joint * X_tree[j];
   wXj = wXj * jXpj.inverse();
 }

 return wXj;
}

And here is the modified version of MultibodyModel:

struct MultibodyModel
{
  int n_bodies{};
  std::vector<int> parent{};
  std::vector<SpatialMatrix> X_tree{};
  std::vector<JointModel> joint_models{};
  
  ...
};

Congratulations! We’ve made significant progress in building a model for arbitrary robotic manipulators. Our model now includes enough information to write a usable forward kinematics algorithm.

However, while joint transformation matrices provide the spatial relationship between connected links, these alone are not sufficient.

Let’s revisit the forward_kinematics code snippet. What if we want to calculate the velocity (twist) of the link?

using PoseAndVelocity = std::pair<SpatialMatrix, SpatialVector>;

PoseAndVelocity forward_kinematics(MultibodyModel const &model, VectorX const &q) {
  auto const &[n_bodies, parent, X_tree, joint_models, ...] = model;

  SpatialMatrix jXpj{ SpatialMatrix::Zero() };
  SpatialMatrix wXj{ SpatialMatrix::Identity() };
  SpatialVector V{ SpatialVector::Zero() };

  for (int j = 0; j < n_bodies; ++j) {
    auto X_joint = joint_models[j].joint_transform();
    jXpj = X_joint * X_tree[j];
    wXj = wXj * jXpj.inverse();
    V = ????;
  }

  return { wXj, V };
}

The variable V here represents the velocity of the j-th link expressed in the local frame ${}^{j}V_{j}$ . To compute V, we can use a simple recursive formula:

where $V_{\text{joint}}$ is the spatial velocity of the child link relative to the parent link caused by joint motion. However, at this point, we only have the joint velocity $\dot{q}$ , so we need to extend our joint model to allow mapping of $\dot{q}$ to $V_{\text{joint}}$ .

In Featherstone’s notation, the concept of Motion Subspaces is used for this purpose. The motion subspace of a joint is the transformation matrix $S$ that maps the joint velocity $\dot{q}$ to the spatial velocity $V_{\text{joint}}$ of the child link relative to the parent link:

Motion Subspace is a somewhat advanced concept, but for the most popular joint types with one degree of freedom, such as revolute and prismatic joints, the motion subspace matrix has a dimension of $6 \times 1$ and essentially represents the Screw Axis of motion.

The intuition behind the screw axis is as follows: suppose we have a link A in motion due to its corresponding joint movement, with some spatial velocity $V_A$ . The screw axis $S$ is a normalized representation of that spatial velocity such that:

where $\dot{q}$ is the corresponding joint velocity. This concept is similar to rotational motion, where angular velocity $\omega$ can be viewed as $\hat{\omega}\theta$ , with $\hat{\omega}$ being the unit rotation axis and $\theta$ the rate of rotation about that axis. A twist $V$ can be interpreted in terms of a screw axis $S$ and a joint velocity $\dot{q}$ about the screw axis.

For the revolute joint (around Z-axis):

1. The first three components of the screw axis represent the axis of rotation $\hat{\omega}$
2. The last three components are zero.

For the prismatic joint (along Z-axis):

1. The first three components of the screw vector represent the axis of rotation $\hat{\omega}$ and are zero.
2. The last three components represent the axis of displacement.

In the general case, the definition for the screw axis is as follows:

(a) If $\omega \neq 0$ then:

$S = \frac{V}{\|\omega\|} = \left(\frac{\omega}{\|\omega\|}, \frac{v}{\|\omega\|}\right)$

The screw axis $S$ is simply $V$ normalized by the length of the angular velocity vector. The angular velocity about the screw axis is $\hat{\theta} = \|\omega\|$ , such that $S\hat{\theta} = V$ .

(b) If $\omega = 0$ then:

$S = \frac{V}{\|v\|} = \left(0, \frac{v}{\|v\|}\right)$

The screw axis $S$ is simply $V$ normalized by the length of the linear velocity vector. The linear velocity along the screw axis is $\hat{\theta} = \|v\|$ , such that $S\hat{\theta} = V$ .

So we will expand our JointModel with a method that returns the screw axis information corresponding to the type of the joint:

class JointModel {
public:
    virtual SpatialMatrix joint_transform(double theta) const = 0;
    virtual SpatialVector screw_axis() const = 0;
};

class RevoluteJoint : public JointModel {
public:
    explicit RevoluteJoint(JointAxis axis) : axis(axis) {}

    ...
  
   	SpatialVector screw_axis() const override {
        SpatialVector result = SpatialVector::Zero();
        switch (axis_) {
        case JointAxis::X:
            result(0) = 1;
            break;
        case JointAxis::Y:
            result(1) = 1;
            break;
        case JointAxis::Z:
            result(2) = 1;
            break;
       	...
        }
      
        return result;
    }
private:
    JointAxis axis;
};

...

Having modified the code, we can write the final version of forward_kinematics:

using PoseAndVelocity = std::pair<SpatialMatrix, SpatialVector>;

PoseAndVelocity forward_kinematics(MultibodyModel const &model, VectorX const &q) {
  auto const &[n_bodies, parent, X_tree, joint_models, ...] = model;

  SpatialMatrix jXpj{ SpatialMatrix::Zero() };
  SpatialMatrix wXj{ SpatialMatrix::Identity() };
  SpatialVector S;
  SpatialVector V{ SpatialVector::Zero() };

  for (int j = 0; j < n_bodies; ++j) {
    auto X_joint = joint_models[j].joint_transform();
    S = joint_models[j].screw_axis();
    jXpj = X_joint * X_tree[j];
    wXj = wXj * jXpj.inverse();
    auto V_joint = S[i] * qd(i);
    V = jXpj * V + V_joint;
  }

  return { wXj, V };
}

One may notice that in Featherstone’s book, a different implementation is suggested: the jcalc function.

void jcalc(const JointType& jtyp, double q, SpatialMatrix& Xj, SpatialVector& S) {
   std::string code = jtyp.code;
   
   if (code == "Rx") {                // revolute X axis
       Xj = rotx(q);
       S << 1, 0, 0, 0, 0, 0;
   } else if (code == "Ry") {         // revolute Y axis
       Xj = roty(q);
       S << 0, 1, 0, 0, 0, 0;
   } else if (code == "R" || code == "Rz") { // revolute Z axis
       Xj = rotz(q);
       S << 0, 0, 1, 0, 0, 0;
   } else if (code == "Px") {         // prismatic X axis
       Xj = xlt(Eigen::Vector3d(q, 0, 0));
       S << 0, 0, 0, 1, 0, 0;
   } else if (code == "Py") {         // prismatic Y axis
       Xj = xlt(Eigen::Vector3d(0, q, 0));
       S << 0, 0, 0, 0, 1, 0;
   } else if (code == "P" || code == "Pz") { // prismatic Z axis
       Xj = xlt(Eigen::Vector3d(0, 0, q));
       S << 0, 0, 0, 0, 0, 1;
   } else if (code == "H") {          // helical (Z axis)
       Xj = rotz(q) * xlt(Eigen::Vector3d(0, 0, q * jtyp.pitch));
       S << 0, 0, 1, 0, 0, jtyp.pitch;
   } else if (code == "r") {          // planar revolute
       Xj = plnr(q, Eigen::Vector2d(0, 0));
       S << 1, 0, 0;
   } else if (code == "px") {         // planar prismatic X axis
       Xj = plnr(0, Eigen::Vector2d(q, 0));
       S << 0, 1, 0;
   } else if (code == "py") {         // planar prismatic Y axis
       Xj = plnr(0, Eigen::Vector2d(0, q));
       S << 0, 0, 1;
   } else {
       throw std::runtime_error("unrecognized joint code '" + code + "'");
   }
}

Some might find this implementation more straightforward and efficient. However, I believe modeling it as a class is more illustrative and generally preferable for readability.

Everything needed to extract JointModel from URDF is in the tag:

<joint name="joint_name" type="revolute">
    <parent link="parent_link_name" />
    <child link="child_link_name" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    <axis xyz="0 0 1" />
</joint>

Usually we only need type and axis attributes.

void add_joint_to_model(MultibodyModel &model, ::urdf::JointConstSharedPtr const &joint)
{
  if (joint->type == ::urdf::Joint::REVOLUTE) {
    auto revolute_joint = RevoluteJoint(map_to_model_axis(joint->axis));
    model.joints.emplace_back(revolute_joint);
  } else if (joint->type == ::urdf::Joint::FIXED) {
    ...
  }
  ...
}

At this point, our MultibodyModel class incorporates:

1. Connectivity information
2. Geometric information
3. Motion information

This allows us to write kinematics algorithms. However, our model currently lacks the inertial properties of links, which are essential for implementing dynamics algorithms such as RNEA (Recursive Newton-Euler Algorithm).

In the next section, we will address this limitation.

Single Link Inertia properties

Currently, our model lacks inertia properties that are critical for calculating forces applied to the links.

Unsurprisingly, a rigid body is characterized by mass and rotational inertia matrix. However, similar to previously discussed quantities, Featherstone’s notation describes this using the spatial inertia matrix.

Spatial inertia matrices, denoted as $I$ , encapsulate both the mass and rotational inertia of a rigid body in a single $6 \times 6$ matrix.

The intuition behind spatial inertia matrices is as follows:

• Unified Representation: Spatial inertia combines linear and angular properties, allowing us to treat translation and rotation uniformly.
• Momentum-Centric: Spatial inertia directly relates spatial velocity to spatial momentum.

Therefore, we will enhance our MultibodyModel by adding an array of spatial inertia matrices for each robot link:

struct MultibodyModel
{
  int n_bodies{};
  std::vector<int> parent{};
  std::vector<SpatialMatrix> X_tree{};
  std::vector<JointModel> joint_models{};
  std::vector<SpatialMatrix> I{}; <---
};

Now let’s examine the structure of each matrix $I$ in detail:

As mentioned earlier, $I$ incorporates the body’s mass and rotational inertia. Mass is straightforward, represented by a single number. However, rotational inertia matrices are more complex due to their dependence on the reference point and frame in which they are expressed. It is important to consider the rules for expressing rotational inertia in different reference frames when deriving the spatial inertia matrix.

When the spatial inertia matrix is expressed relative to the Center of Mass (CoM), it has a nice, simple structure:

where

• ${}^{C}I_{C}$ represents the spatial inertia relative to CoM expressed in the CoM frame
• ${}^{C}\bar{I}_{C}$ - is the rotational inertia relative to CoM, also expressed in the CoM frame.
• $m$ denotes the mass of the link
• $I_{3\times3}$ is the identity matrix

These properties, ${}^{C}I$ and $m$ are typically provided in the robot’s CAD model and often transferred to the corresponding URDF.

However, a critical question arises: In what frame and about which reference point should $I$ be expressed?

In Featherstone’s notation, the link frame, usually not aligned with the CoM frame, is commonly used. Additionally, the reference point of rotation is often shifted from the CoM. Consider a revolute joint where the axis of rotation is displaced from the CoM. In our model, the reference point of rotation coincides with the origin of the link frame of the i-th link, $F_i$ .Thus, we need to re-express the spatial inertia matrix in $F_i$ . We have to both apply rotation to re-express it in the frame $F_i$ instead of $C$ and shift the reference point from the CoM to the $F_i$ origin.

Rotation affects only the rotational inertia matrix $\bar{I}$ , and its transformation is defined by:

${}^{F_i}\bar{I}_{C} = R {}^{C}\bar{I}_{C} R^T$ ,

where $R$ is the rotation matrix from the CoM frame $C$ to link frame $F_i$ : ${}^{F_i}R_{C}$ .

To change the reference point, we apply Steiner’s theorem as follows: $${}^{F_i}I = \begin{bmatrix} ^{F_i}\mathbf{\bar{I}_{C}} + m[^{F_i}\mathbf{p}_C][^{F_i}\mathbf{p}_C]^T & m[^{F_i}\mathbf{p}_C] \\ m[^{F_i}\mathbf{p}_C]^T & m\mathbf{I}_{3\times3} \end{bmatrix}$$ where $^{F_i}\mathbf{p}_C$ is the displacement of the CoM in $F_i$ , and $[^{F_i}\mathbf{p}_C]$ is the skew-symmetric representation of that vector.

Although the derivation of this transformation is beyond the scope of this article, the concept remains clear: starting with an inertia matrix expressed in the CoM frame, we aim to express it in the link’s frame by applying rotation and utilizing Steiner’s theorem.

Again, let’s examine how extraction from URDF may look:

{
  ...
    
  ::urdf::Inertial const &Y = *link->inertial;
  auto [ipC, iRC] = get_com_position_and_orientation(Y);

  RotationalInertia C_rotI_C;
  C_rotI_C <<
    Y.ixx, Y.ixy, Y.ixz,
    Y.ixy, Y.iyy, Y.iyz,
    Y.ixz, Y.iyz, Y.izz;

  RotationalInertia const i_rotI_C = iRC * C_rotI_C * iRC.transpose();

  auto I = apply_steiners_theorem(i_rotI_C, ipC, Y.mass);
  model->I.emplace_back(I);
  
  ...
}

SpatialMatrix apply_steiners_theorem(RotationalInertia &i_rotI_C, Vector3 &ipC, double mass)
{
  SpatialMatrix I{ SpatialMatrix::Zero() };
  SkewSymmetric const com_so3 = so3(ipC);
  SkewSymmetric const com_so3_T = com_so3.transpose();
  I.topLeftCorner<3, 3>() = i_rotI_C + mass * com_so3 * com_so3_T;
  I.topRightCorner<3, 3>() = mass * com_so3;
  I.bottomLeftCorner<3, 3>() = mass * com_so3_T;
  I.bottomRightCorner<3, 3>() = mass * Matrix3::Identity();

  return I;
}

Wrap up

As our exploration draws to a close, we have successfully constructed a model capable of describing arbitrary robotic manipulators.

struct MultibodyModel
{
  int n_bodies{};
  std::vector<int> parent{};
  std::vector<SpatialMatrix> X_tree{};
  std::vector<JointModel> joint_models{};
  std::vector<SpatialMatrix> I{};
};

View full source code

Here are the key components of our model:

• n_bodies - number of links excluding the base link of the manipulator
• parent - array of parent indices representing the connectivity of the robot’s links
• X_tree - provides geometric information for positioning of the links in space
• joint_models - describes relative movement of links
• I - spatial inertia matrices for each robot link

Below is a full pseudocode illustrating how this model can be constructed from URDF.

MultibodyModel parse_urdf(std::string const &filename, std::optional<spdlog::logger> const &logger)
{
  ::urdf::ModelInterfaceSharedPtr const urdfTree = ::urdf::parseURDFFile(filename);
  auto builder = MultibodyModel::create_from_urdf().set_root(urdfTree);

  if (logger) { builder.set_logger(*logger); }

  std::queue<::urdf::LinkConstSharedPtr> link_queue{};

  for (auto const &link : urdfTree->getRoot()->child_links) { link_queue.push(link); }

  while (!link_queue.empty()) {
    ::urdf::LinkConstSharedPtr const link = link_queue.front();
    link_queue.pop();
    for (auto const &child_link : link->child_links) { link_queue.push(child_link); }

    builder.add_link_and_joint_to_model(link, link->parent_joint);
  }

  return builder.build();
}

MultibodyModelBuilder &MultibodyModelBuilder::add_link_and_joint_to_model(
  ::urdf::LinkConstSharedPtr const &link,
  ::urdf::JointConstSharedPtr const &joint)
{
  if (joint->type == ::urdf::Joint::FIXED and not is_end_effector(link)) { return *this; }
  
  ...
    
  add_joint_to_model(*model, joint);

  int const parent_idx = get_parent_idx(link);
  model->parent.emplace_back(parent_idx);
  (*link_name_to_idx)[link->name] = model->n_bodies++;

  SE3 T = Tinv(urdf_pose_to_SE3(joint->parent_to_joint_origin_transform));
  auto X = Ad(T);

  model->X_tree.emplace_back(X);

  ::urdf::Inertial const &Y = *link->inertial;
  auto [ipC, iRC] = get_com_position_and_orientation(Y);

  RotationalInertia C_rotI_C;
  C_rotI_C <<
    Y.ixx, Y.ixy, Y.ixz,
  	Y.ixy, Y.iyy, Y.iyz,
  	Y.ixz, Y.iyz, Y.izz;

  RotationalInertia const i_rotI_C = iRC * C_rotI_C * iRC.transpose();

  auto I = apply_steiners_theorem(i_rotI_C, ipC, Y.mass);
  model->I.emplace_back(I);

  return *this;
}

View full source code

Implementation in Pinocchio

One of the most handy and production-ready libraries for robot dynamics modeling is Pinocchio.

Upon examining the code of Pinocchio, we will find that the model class contains fields that are either identical or very similar to those previously described.

template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct ModelTpl
: serialization::Serializable<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
, NumericalBase<ModelTpl<_Scalar, _Options, JointCollectionTpl>>
{
  ...

  /// \brief Number of bodies.
  int nbodies;

  /// \brief Vector of spatial inertias supported by each joint.
  InertiaVector inertias;

  /// \brief Vector of joint placements: placement of a joint *i* wrt its parent joint frame.
  SE3Vector jointPlacements;

  /// \brief Vector of joint models.
  JointModelVector joints;

  ...
};

View full source code

However, it is important to note that Pinocchio’s implementation is significantly more complex. It supports non-trivial joint types, custom frames, the inertial properties of motors, and much more.

Conclusion

This blog post marks the beginning of my deep dive into the world of robotics modeling, focusing on building Rigid Body Dynamics (RBD) algorithms from scratch and analyzing their implementation in dynamics libraries such as Pinocchio, Drake, and MuJoCo. We have outlined the core components of a Rigid Body System’s model and observed how they are reflected within Pinocchio. Moving forward, in future articles, I plan to cover classical algorithms such as RNEA (Recursive Newton-Euler Algorithm), CRBA (Composite Rigid Body Algorithm), and ABA (Articulated Body Algorithm) with an emphasis on intuitive understanding and practical code implementation rather than rigorous mathematical detail. Additionally, I will dig into the implementation details inside Pinocchio to explore what it really takes to transform theoretical concepts into real, functional software.

Further reading

1. Rigid body dynamics algorithms by Roy Featherstone

   This book is the primary resource on the subject, but it’s quite advanced and written as a handbook for practitioners. While comprehensive, it may be challenging for beginners without additional context.

2. MEE5114 by Professor Wei Zhang

   This brilliant course offers an excellent introduction to the concepts covered in Featherstone’s book. It provides a more accessible approach to understanding rigid body dynamics algorithms.

3. Modern Robotics: Mechanics, Planning, and Control by Kevin M. Lynch and Frank C. Park

   This book covers many of the same topics as Featherstone’s work but is generally easier to comprehend. It’s an excellent resource for building a strong foundation in robotics concepts, including those related to rigid body dynamics.