Compact C++ articulated rigid body simulation library implementing:

• Forward dynamics via the Articulated Body Algorithm (ABA) — $O(N_B)$,
• Forward dynamics via the Composite Rigid Body Algorithm (CRBA) — $O(N_B^2)$,
• Inverse dynamics via the Recursive Newton–Euler Algorithm (RNEA) - $O(N_B)$,
• Collision detection and spatial impulse-based contact resolution, and
• Unified Robot Description Format (URDF) parser for model import and export.

Includes end-to-end automatic differentiation, enabling system identification, optimisation, and machine learning applications.

⚡ Quick Start ⚡

If you have CMake and a C++23 compliant compiler, you can clone, download all dependencies, and build ARB with a single command:

git clone https://github.com/wbyates777/Articulated-Rigid-Body.git && cd Articulated-Rigid-Body && mkdir build && cd build && cmake .. && make -j8

Demos

Simulating doors with revolute and prismatic joints:

https://youtu.be/O9h_phDP_tk

Rigid body simulation using collision detection and contact resolution:

https://youtu.be/g1jMEpu1sl8

Introduction

ARB is a compact implementation of a differentiable spatial algebra, designed for use in computer graphics, simulation, and robotics.

It combines:

• Spatial algebra (6D motion and force vectors),
• Automatic differentiation (AD) across the full simulation pipeline (including contacts),
• Efficient rigid body dynamics algorithms (ABA, CRBA, RNEA),
• Collision detection (GJK/EPA) and impulse based contact resolution (PGS solver, warm start), and
• URDF file parser for model import and export.

This allows simulation of articulated systems ranging from simple mechanisms to fully-jointed characters, with support for advanced applications such as system identification and model-based control.

The spatial algebra is used to implement three important rigid body dynamics algorithms: articulated-body algorithm (ABA), the composite rigid body algorithm (CRBA), and the recursive Newton-Euler algorithm (RNEA) (see Featherstone 2008). The algebra is also used to implement a spatial impulse based, collision resolution algorithm. All the algorithms have been tested extensively in a graphical environment.

The implementations of ABA, CRBA, and RNEA presented here are based on those in the well known robotics library RBDL, but are intended for use in computer graphics. They allow a programmer to handle, in a physically consistent manner, rigid bodies and articulated rigid bodies. In the context of graphics these articulated bodies could range from a fully-jointed humanoid character, to a simple hinged mechanism such as a door.

Spatial algebra combines the 3D-linear and 3D-angular components of rigid body physics and provides a compact notation that significantly reduces the "volume of algebra by at least a factor of 4 compared with standard 3D vector notation" (Featherstone 2008). Spatial algebra also significantly reduces the complexity of the implementation.

Automatic differentiation is provided by the autodiff library. Thus the algebra, the ABA, and the RNEA, are completely differentiable. This end-to-end differentiability facilitates the application of advanced optimisation, machine learning techniques, and System Identification (SI).

The BContactManager class can detect collisions between (pairs of) rigid bodies and can resolve multiple 'contacts' by employing the physical concept of spatial impulse.

This code is written in C++23 and depends primarily on STL and the header-only GLM. Additionally, the header-only autodiff library is required for automatic differentiability, and the openGJK or libccd library for collision detection.

Key Features

• Articulated-body algorithm (ABA) - $O(N_B)$ forward dynamics for kinematic trees,
• Composite Rigid-Body Algorithm (CRBA) - $O(N_B^2)$ calculate joint-space inertia matrix $M(q)$ for kinematic trees.
• Recursive Newton-Euler algorithm (RNEA) - $O(N_B)$ inverse dynamics for kinematic trees,
• Spatial algebra implementation (header-only),
• End-to-end automatic differentiability using autodiff (header-only),
• Collision Resolution – spatial impulses and GJK/EPA contact manifolds,
• Unified Robot Description Format (URDF) import and export,
• Minimal dependencies STL, GLM, (autodiff, openGJK, and libccd optional).

Background

Forward and Inverse Dynamics of Kinematic Trees

Trees (and chains) of kinematic equations are used in robotics, computer graphics, and animation. In robotics forward kinematics refers to the use of kinematic equations to compute the position of an end-effector, such as a jointed robotic arm, from specified values for the joint parameters (see figure 1). The reverse calculation, that computes the joint parameters that achieve a specified arm position, is known as inverse kinematics.

Figure 1. A simple jointed robot arm.

The BDynamics class contains an implementation of:

• the articulated-body algorithm (ABA),
• the composite rigid-body algorithm (CRBA), and
• the recursive Newton-Euler algorithm (RNEA).

These algorithms are presented in:

"Rigid Body Dynamics Algorithms" (RBDA), R. Featherstone, Springer, 2008.

ABA is an example of a propagation algorithm, and it is the fastest known algorithm for calculating the forward dynamics of a kinematic tree with a computational complexity of $O(N_B)$, where $N_B$ is the number of bodies/joints. This is the theoretical minimum for solving the forward dynamics problem (see RBDA, Section 7.3).

CRBA also calculates forward dynamics. In contrast to ABA, it forms an equation of motion for the whole system, and then solves it for the acceleration variables. Although CRBA is $O(N_B^2)$ it can match or slightly exceed the speed of the $O(N_B)$ ABA on trees with only a few bodies, or that are branched enough to have a small depth.

RNEA calculates the inverse dynamics of a kinematic tree. It is the simplest, most efficient known algorithm for trees, and also has a computational complexity of $O(N_B)$ (see RBDA, Section 5.3).

ARB supports the following joints:

Spatial Algebra

The algorithms described above are formulated and implemented using spatial algebra (see RBDA, Chapter 2). Spatial algebra employs 6D vectors that combine the 3D linear and 3D angular aspects of rigid-body motion. Linear and angular velocities (or accelerations) are combined to form spatial motion vectors, while linear forces and torques are combined to form spatial force vectors. By convention, spatial velocity vectors are called twists, while spatial force vectors are called wrenches.

Spatial algebra significantly reduces the "volume of algebra by at least a factor of 4 compared with standard 3D vector notation" (see RBDA, Section 1.2). It also significantly reduces the complexity of the implementation of rigid body and articulated rigid body physics. For example, the following 'hello world' program performs Newton-Euler integration:

   #include "BSpatialAlgebra.h"
    
   int 
   main( void )
   {
      // set up single body - a sphere -  
      double mass = 100.0, radius = 0.5; 
      BMatrix3 I_o((2.0/5.0) * mass * (radius * radius)); 
     
      BRBInertia I( BInertia(mass, I_o) );
      BMatrix6 invI = arb::inverse(I);
  
      // set initial position, velocity and acceleration (angular, linear)
      BVector6 pos(B_ZERO_3, 20.0, 50.0, 3.0), vel(0.0), acc(0.0);
      
      // apply an angular force of 1.0 around Y axis and a linear force of 100.0 along Z axis
      BVector6 force(0.0, 1.0, 0.0,  0.0, 0.0, 100.0);
  
      double T = 0.0, dt = 0.01;
      std::cout << T << " position is " << pos << std::endl;
      for (int t = 0; t < 500; ++t)
      {
          // Newton-Euler (see Featherstone, Section 2.15, eqn 2.72, page 36).
          acc = invI * (force - arb::crossf(vel, I * vel)); 
          vel += acc * dt;
          pos += vel * dt;
          T += dt;
      }
      std::cout << T << " position is " << pos << std::endl; // angles in radians!
      return EXIT_SUCCESS;
   }

This produces the output:

0.0 position is 0.00000000 0.00000000 0.00000000 20.00000000 50.00000000 3.00000000 
5.0 position is 0.00000000 1.25250000 0.00000000 15.23282410 50.00000000 13.89482750 

The acceleration due to an actual, physical force is calculated by the term acc = invI * force. The extra term arb::crossf(vel, I * vel) is called the bias force. The bias force represents an inertial force; a so called fictitious force such as centrifugal, Coriolis, or Euler force. The inertial force is necessary for describing motion correctly.

Automatic Differentiation

This library also supports Automatic Differentiation (AD). Adding automatic differentiation to the spatial algebra library means that the algebra, ABA, CRBA, RNEA, and collision resolution are completely differentiable. Unlike numerical differentiation (finite differences), which is computationally expensive and prone to truncation errors, AD uses the chain rule to propagate exact analytical derivatives through the code at the machine level. This end-to-end differentiability facilitates the application of more advanced optimisation and machine learning techniques such as real-time Model Predictive Control (MPC), gradient-based trajectory optimisation, or analytical System Identification (SI).

For example, consider System Identification (SI). System Identification is the process of constructing a mathematical model of a dynamic system, using analytical methods, based on observed input and output data. While forward dynamics (ABA/CRBA) predicts how a system moves given its physical parameters, SI reverses this process: it can recover the underlying physical parameters (mass, centre-of-mass, inertia tensor) of each body by observing the system's motion (see ARB example 6 in main.cpp). SI allows researchers to synchronise a simulation with the real world. Additionally, SI can be used for the estimation of non-conservative forces such as joint friction and damping, which are traditionally difficult to calibrate manually.

Collision Detection and Impulse Based Resolution

The task of detecting collisions between rigid bodies is non-trivial. It is typically subdivided into two phases: broad phase and narrow phase. Broad-phase consists of detecting intersections between oriented bounding boxes (OBB) using the Separating Axis Theorem (SAT). Computationally, this is relatively efficient. The narrow-phase detects intersections between mesh colliders represented by polytopes (convex hulls) using the GJK algorithm. This is very precise but computationally more expensive.

ARB provides two options for narrow-phase collision detection:

• the default libccd backend, or
• an implementation of openGJK.

Libccd is a well-tested C library, with support for standard primitives such as boxes and spheres, as well as complex arbitrary shapes represented as polytopes (convex hulls). It is released under a permissive license, compatible with the MIT license used here. However it does not use differentiable types, and so, it is not possible to differentiate across a contact, that is, to compute post-collision derivatives for interacting bodies. The openGJK C++ implementation presented here does employ differentiable types. However it only supports polytopes; although boxes can be represented as 8-point polytopes, analytical primitives like spheres are not natively supported. Furthermore, openGJK is released under the GPLv3 license, and using this module will subject the entire project to GPLv3 copyleft terms.

Collisions are resolved by calculating the spatial impulses resulting from a contact and using these impulses to update the object's velocities so that they separate appropriately (see RBDA, Section 11.7).

The BContactManager class utilizes an iterative Projected Gauss-Seidel (PGS) solver to resolve multiple contact constraints simultaneously. It combines Baumgarte Stabilization for overlap recovery with a circular friction cone (Coulomb friction) model, providing more realistic sliding and sticking behavior than a box-friction approximation. It also uses impulse caching (warm starting) across frames to ensure stability and eliminate jitter in resting or stacked objects.

https://youtu.be/g1jMEpu1sl8

Unified Robot Description Format (URDF)

The Unified Robot Description Format (URDF) is an XML-based industry standard used to define the physical configuration of a robot, acting as the universal language for the Robot Operating System (ROS) ecosystem. It specifies a robot’s kinematic tree, including the hierarchy of links, the types of joints connecting them, and their respective physical properties such as mass, centre of mass, and inertia tensors.

By supporting URDF, ARB can load complex, real-world robot models, such as the ur5 or tiago, directly from standard industry files. The library parses these XML descriptions to automatically construct the internal spatial inertia matrices and joint transforms required for the ABA, CRBA and RNEA algorithms. These models can also be exported to other URDF compliant systems for cross-validation.

Implementation

The implementations presented here, are intended for use in computer graphics, and are based on those in the RBDL library. Alternative implementations can be found in the RBDyn library, and the Pinocchio library (see below for links). We intentionally use similar variable names and the same object structure and hierarchy as RBDL. This facilitates numerical comparison testing. Some variables have been moved to their appropriate classes and accessor methods have been added throughout. This improves encapsulation and readability.

RBDL, RBDyn, and Pinocchio employ the Eigen3 linear algebra library. Eigen3 supports all matrix sizes, from small fixed-size matrices to arbitrarily large dense matrices, and even sparse matrices. This code does not depend on Eigen3, and instead relies on the light-weight, header-only GLM library. GLM (OpenGL Mathematics) is based on the OpenGL Shading Language (GLSL) specifications, and provides a highly optimised implementation of 3D linear algebra primitives.

The spatial algebra implementation (though not the algorithms) is also header-only, and depends solely on STL and the 3D GLM types:

glm::vec3, glm::mat3, glm::quat,

and the GLM functions:

glm::cross(u,v), glm::dot(u,v), glm::outerProduct(u,v), glm::transpose(m), glm::inverse(m), glm::mat3_cast(q).

Given the small number of 3D functions and types employed, it is straightforward to convert back to Eigen3 (although see the note on Eigen3's and GLM's row-major, column-major differences), or replace GLM with a high(er)-performance alternative, such as a custom std::simd linear algebra library. The design goal here is to appropriate GLM's clean and intuitive syntax, while remaining (mostly) agnostic about the underlying implementation.

In order to implement automatic differentiation it is sufficient to #include "BAutodiff.h" in the BSpatialTypes.h file. BAutodiff.h is a wrapper for the header-only autodiff library (see below). This enables the automatic computation of derivatives in an efficient and intuitive manner. It should be noted that the calculated derivatives are exact (to machine precision) and not approximated, as is the case for finite difference methods.

The collision detection component depends on two external libraries libccd and openGJK (see below). Libccd is a C library that implements a variant of the Gilbert–Johnson–Keerthi algorithm and the Expanded Polytope Algorithm (EPA). OpenGJK is a more modern and efficient C++ implemenation of GJK/EPA. The OpenGJK used here has been rewritten to use GLM types in order to support autodiff types, and automatic differentiation.

The URDFManager class depends on TinyXML2. TinyXML2 is a simple, small, and efficient open-source C++ XML parser that can be easily integrated into other programs. It is a rewrite of the original TinyXML, designed to be more memory-efficient, use fewer allocations, and run faster. As TinyXML2 consists of only two files, for simiplicity these are included in this codebase.

Validation

This implementation has been numerically validated against RBDL v3.3.1 and tested extensively in a graphics environment. ARB has been tested against standard industrial robot models including:

ur5.urdf,  iiwa7.urdf,  iiwa14.urdf,  r1.urdf,  go1.urdf,  and  tiago_dual-test.urdf

The results of testing tiago_dual-test.urdf, a robotic arm with 74 components and 33 degrees of freedom (taken from the RBDL documentation) are shown in the table below. The table shows the accelerations for each component calculated using the ABA by RBDL and ARB. Notice that the maximum difference across all components is 0.00022200.

The file BSpatialChecks.cpp includes 52 consistency checks, tested on random examples, that verify basic analytical relationships and identities. These checks cover spatial transforms, cross products and inertia operations, ensuring algebraic self-consistency. For example, in Lie group theory, for any given transform $X$ and twist $u$, the Adjoint Identity

     exp(Adjoint(X) * u) == X * exp(u) * X^{-1}  

is a fundamental property that links adjoints, spatial transforms, inverses, and the exp mapping.

The RNEA is a linear mapping from accelerations to torques, while the ABA is the inverse mapping, and so, given some joint accelerations qddot, their composition should satisfy the identity:

     ABA(q, qdot, RNEA(q, qdot, qddot)) ==  qddot  

The test example 5 in dynamics_test (see main.cpp) demonstrates this.

Performance

To measure perfomance ARB has been benchmarked against RBDL v3.3.1.

The table below shows the execution times in milliseconds of forward dynamics (ABA/CRBA) and inverse dynamics (RNEA) algorithms over 100,000 iterations using the highly jointed, industrial robot configuration tiago_dual-test.urdf.

Notes

• Zero Run-Time Allocations: Tests conducted on pre-allocated data structures.
• Compilation Environment: C++23 clang optimized with flags: -O3 -DNDEBUG -DGLM_FORCE_INTRINSICS -DGLM_FORCE_DEFAULT_ALIGNED_GENTYPES -march=native.

Despite ARB's lightweight footprint when intrinsics (SIMD) is enabled, it achieves near-parity (within single-digit percentage variations) against a heavily vectorized (Eigen3) and highly optimised industry standard (RBDL). Having established some benchmark timings future development cycles will focus on closing the remaining performance gap.

Build Instructions

On a platform that supports cmake you can use the CMakeList.txt file included in this project. Simply cd to the directory where you have saved this project and enter:

mkdir build ; cd build ; cmake .. ; make

Cmake will take care of installing the GLM, autodiff, and libccd libraries, and building the executable 'dynamics_test'. If you are not using cmake these libraries can be downloaded directly from github (see links below).

The minimum compiler requirement is now C++23. This is due to the improved constexpr handling in C++23. If you do not use GLM_FORCE_INTRINSICS you can use C++20. If you remove (some) constexpr definitions, then my code will compile under C++20, and 'almost' compile under C++17. GLM and autodiff are both C++17 compliant. See the source for details on the remaining minor C++17 changes.

Libraries

Dependencies

GLM - https://github.com/g-truc/glm

autodiff - https://github.com/autodiff/autodiff

CCD - https://github.com/danfis/libccd

openGJK - https://github.com/MattiaMontanari/openGJK

TinyXML2 - https://github.com/leethomason/tinyxml2

External

RBDL - https://github.com/rbdl/rbdl

RBDyn - https://github.com/jrl-umi3218/RBDyn

Pinocchio - https://stack-of-tasks.github.io/pinocchio/

Notes

This project is licensed under the MIT License.

Please click here: