This guide provides a comprehensive, step-by-step approach to implementing a new joint type in Pinocchio. It covers mathematical foundations, core implementation, parser integration, and testing requirements.
Table of Contents
Overview
Pinocchio Directory Structure
Prerequisites
Step 1: Mathematical Foundation
Step 2: Core Joint Implementation
Step 3: Motion Subspace Specialization
Step 4: Model Graph Integration
Step 5: Testing
Summary Checklist
Overview
Implementing a new joint in Pinocchio involves three major phases:
Core Implementation: Define the joint's kinematics and dynamics
Parser Integration: Enable graph-based model construction and reversable joints
Testing: Verify correctness through comprehensive unit tests
This document guides you through each phase systematically.
Pinocchio Directory Structure
Before diving into implementation, familiarize yourself with Pinocchio's organization. The codebase is structured as follows:
pinocchio/
├── include/pinocchio/ # C++ headers (main implementation)
For simple joints (1-DOF), study Revolute or Prismatic
For Euler-angle joints, study Spherical ZYX
For constrained surfaces, study Ellipsoid
All joint headers are in include/pinocchio/multibody/joint/
Key Mathematical Concepts
A joint is characterized by:
Configuration dimensionnq: Size of the configuration vector
Velocity dimensionnv: Size of the velocity vector
Motion subspace: Maps joint velocity to spatial velocity:
Spatial transform: Placement from parent to child frame
Bias acceleration: Often written as
Step 1: Mathematical Foundation
What You'll Do: Derive the mathematical equations that govern your joint's behavior. You will compute the spatial transform , motion subspace , and bias acceleration using pen-and-paper or symbolic tools.
Goal: By the end of this step, you should have closed-form expressions ready to implement in code.
1.1 Define the Joint Configuration
First, determine your joint's configuration and velocity spaces:
What are the generalized coordinates ?
What is the relationship between and (are they in the same space)?
Important
For non-Euclidean joints (e.g., spherical joints using quaternions), nq may differ from nv. Ensure you understand the Lie group structure.
1.2 Derive the Spatial Transform
Compute the spatial transformation matrix from parent frame p to child frame c:
Where:
is the rotation matrix
is the translation vector
denotes the skew-symmetric matrix
Remarks
The ellipsoid joint has translation and rotation that depends on ellipsoid radii and configuration :
1.3 Derive the Motion Subspace
The motion subspace is the Jacobian of the spatial velocity with respect to joint velocity:
Where the spatial velocity in Pinocchio's convention is:
Note
Pinocchio uses the convention (linear, angular), while some literature (in the Featherstone book) uses (angular, linear). Be consistent with Pinocchio's convention.
Computing (Angular Velocity)
For rotation-based joints, extract angular velocity from:
Where extracts the vector from a skew-symmetric matrix.
Computing (Linear Velocity)
Differentiate the translation with respect to time:
1.4 Derive the Bias Acceleration
The bias acceleration (also called velocity product) is:
Where:
Or equivalently:
Remarks
Use symbolic computation tools (SymPy, Mathematica) to derive these expressions. The math can become complex quickly, especially for .
Sdot = sp.Matrix.zeros(6, 3)
for i in range(3):
for j in range(3):
Sdot[:, i] += sp.diff(S[:, i], q[j]) * qdot[j]
Step 2: Core Joint Implementation
What You'll Do: Create C++ header files defining your joint's JointModel and JointData structures. You will translate your mathematical derivations into efficient C++ code that Pinocchio can use.
All typedef here determines how spatial algebra vectors are represented. Here, JointMotionSubspaceTpl, SE3Tpl and MotionTpl are dense matrices. To take advantage of sparse patterns, they can be specialized (see Section 3 for JointMotionSubspaceTpl specialization). It's generally a good idea to implement the non-sparse version first.
2.3 Define the JointData Struct
JointData stores the joint's runtime state:
template<typename _Scalar, int _Options>
struct JointDataMyJointTpl : public JointDataBase<JointDataMyJointTpl<_Scalar, _Options>>
Use helper functions like computeSpatialTransform, computeMotionSubspace, and computeBias to keep your code modular and readable. Espacially is you have complex formulae. Like for an Ellipsoid Joint.
What You'll Do: Optionally create a custom constraint class to exploit sparse structure in your motion subspace matrix. This step is optional but can significantly improve performance.
When to do this: If your joint's matrix has many zeros or a repeating pattern (like Revolute or Prismatic joints).
When to skip this: If your matrix is dense (like Ellipsoid), use the generic JointMotionSubspaceTpl and skip to Step 4.
3.1 When to Specialize
If your joint's motion subspace has a sparse or structured pattern, you can define a custom constraint class to exploit this structure for performance.
Examples:
Revolute joint: (single non-zero entry)
Prismatic joint:
Spherical joint: Block-diagonal structure
When NOT to specialize:
Dense 6×n matrices without obvious structure (use JointMotionSubspaceTpl)
3.2 Implementing a Custom Constraint
Create a new struct inheriting from JointMotionSubspaceBase:
Look at existing specialized constraints like JointModelRevoluteTpl for inspiration.
Step 4: Model Graph Integration
What You'll Do: Enable your joint to work with Pinocchio's graph-based parser, which is used to load URDF files and build models from Python. You will create a graph joint struct and implement visitor functions.
Implement converters to translate configurations between the original and reversed joint parameterizations.
Note
q_source is the configuration in the original model, q_target is the configuration in the converted model (e.g., with a reversed joint). Same for velocities (v_source, v_target).
Joint reversal can be non-trivial. For example, the Ellipsoid joint cannot be reversed because its motion subspace is defined in a specific frame, the parent frame. Only implement reversal if you can get the spatial transform of the joint in the child frame by modifying .
What You'll Do: Write comprehensive unit tests to verify your joint works correctly. Start by testing the joint itself (kinematics, dynamics), then test graph integration.
Files to Create/Modify:
unittest/joint-<name>.cpp (create new test file)
unittest/CMakeLists.txt (register your test)
unittest/model-graph.cpp (add graph test)
unittest/model-configuration-converter.cpp (add converter test, if reversible)
unittest/serialization.cpp (add serialization test, modify only if your test take parameters)
unittest/all-joints.cpp (add to joint list, modify only if your test take parameters)
unittest/joint-generic.cpp (add to generic tests, modify only if your test take parameters)
unittest/finite-differences.cpp (add to finite-diff tests, modify only if your test take parameters)
Testing Priority:
First: Test the joint in isolation (kinematics, , , dynamics)
Second: Test graph integration (if you implemented Step 4)
Third: Test converters and serialization
You should create comprehensive tests covering:
Joint kinematics: M, S, v, c
Joint dynamics: RNEA, ABA
Equivalence tests: Compare with composite joints or analytical solutions
Model graph: Graph construction and conversion
Serialization: Ensure the joint can be saved/loaded
5.1 Create Joint Unit Tests
Location: Create a new file at unittest/joint-<name>.cpp
![$q = [q_0, q_1, \ldots, q_{n-1}]$](form_211_dark.svg)
![\[{^p}X_c(q) = \begin{bmatrix} R(q) & 0 \\ -R(q)p(q)^\times & R(q) \end{bmatrix}
\]](form_212_dark.svg)
![\[\begin{align} p(q) = \begin{bmatrix} a\sin q_1 \\ -b\sin q_0\cos q_1 \\ c\cos q_0\cos q_1 \end{bmatrix} & & R(q) = R_x(q_0) R_y(q_1) R_z(q_2) \end{align}
\]](form_220_dark.svg)
![\[S = \frac{\partial {^p}v_J}{\partial \dot{q}}
\]](form_221_dark.svg)
![\[{^p}v_J = \begin{bmatrix} \dot{p}(q,\dot{q}) \\ \omega(q,\dot{q}) \end{bmatrix}
\]](form_222_dark.svg)
![$[v, \; \omega]^\top$](form_223_dark.svg)
![$[\omega, \; v + p \times \omega]^\top$](form_224_dark.svg)
![\[\omega = \text{axial}(\dot{R}(q)R(q)^T)
\]](form_226_dark.svg)
![\[\dot{p} = \frac{dp}{dt} = \frac{\partial p}{\partial q}\dot{q}
\]](form_229_dark.svg)
![\[c = \dot{S}\dot{q}
\]](form_230_dark.svg)
![\[\dot{S} = \sum_{i=0}^{n_v-1} \frac{\partial S}{\partial q_i}\dot{q}_i
\]](form_231_dark.svg)
![\[\dot{S} = \frac{\partial^2 {^p}v_J}{\partial q \partial \dot{q}} \dot{q}
\]](form_232_dark.svg)
![$S = [0, 0, 0, 0, 0, 1]^T $](form_235_dark.svg)
![$S = [1, 0, 0, 0, 0, 0]^T $](form_236_dark.svg)
1.14.0