Tags: #Pinocchio #tutorial #joints #implementation #motionsubspace
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
- OverviewOverview
- Pinocchio Directory StructurePinocchio Directory Structure
- PrerequisitesPrerequisites
- Step 1: Mathematical FoundationStep 1: Mathematical Foundation
- Step 2: Core Joint ImplementationStep 2: Core Joint Implementation
- Step 3: Motion Subspace SpecializationStep 3: Motion Subspace Specialization
- Step 4: Model Graph IntegrationStep 4: Model Graph Integration
- Step 5: TestingStep 5: Testing
- Summary ChecklistSummary 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)
│ ├── multibody/
│ │ ├── joint/ # Joint definitions
│ │ │ ├── fwd.hpp # Forward declarations
│ │ │ ├── joints.hpp # Includes all joint headers
│ │ │ ├── joint-collection.hpp # Joint variant definition
│ │ │ ├── joint-revolute.hpp # Example: Revolute joint
│ │ │ ├── joint-spherical-ZYX.hpp # Example: Spherical ZYX joint
│ │ │ └── joint-<name>.hpp # Your new joint goes here
│ │ ├── model.hpp # Model class
│ │ └── data.hpp # Data class
│ ├── parsers/
│ │ └── graph/ # Graph-based model construction
│ │ ├── joints.hpp # Graph joint definitions
│ │ ├── graph-visitor.hpp # Graph visitors
│ │ └── model-configuration-converter.hxx # Config converters
│ └── serialization/ # Boost serialization support
│ ├── joints-model.hpp
│ └── joints-data.hpp
├── src/ # C++ source files
│ └── parsers/
│ └── graph/
│ ├── model-graph.cpp # Graph construction logic
│ └── model-graph-algo.cpp # Graph algorithms & visitors
├── bindings/python/ # Python bindings
│ ├── multibody/joint/
│ │ ├── joints-models.hpp # Python exposure for JointModel
│ │ └── joints-datas.hpp # Python exposure for JointData
│ └── parsers/graph/
│ └── expose-edges.cpp # Python exposure for graph joints
├── unittest/ # Unit tests
│ ├── joint-<name>.cpp # Your joint tests go here
│ ├── model-graph.cpp # Graph tests
│ ├── model-configuration-converter.cpp
│ └── CMakeLists.txt # Add your test here
└── examples/ # Usage examples
└── <name>-joint-kinematics.py # Your example goes here
Key directories you'll work in:
- include/pinocchio/multibody/joint/ - Core joint implementation (C++ headers)
- include/pinocchio/parsers/graph/ - Graph integration (for URDF support)
- src/parsers/graph/ - Graph visitor implementations (C++ source)
- bindings/python/ - Python bindings
- unittest/ - All tests
- Important
- Most joint code is header-only (in include/), but graph visitors require source files (in src/).
Prerequisites
Before implementing a new joint, ensure you understand:
- Spatial algebra: Featherstone's spatial notation (6D spatial vectors, spatial transforms)
- Joint kinematics: Configuration space, tangent space, motion subspace
- Pinocchio's architecture: JointModel/JointData pattern, traits system
- C++ templates: Pinocchio heavily uses templates for scalar types and options
Available Joints in Pinocchio
Pinocchio already implements many joint types. Study these as references:
Key Mathematical Concepts
A joint is characterized by:
- Configuration dimension nq: Size of the configuration vector \(q\)
- Velocity dimension nv: Size of the velocity vector \(\dot{q}\)
- Motion subspace \(S(q)\): Maps joint velocity to spatial velocity: \(v_J = S(q)\dot{q}\)
- Spatial transform \(M(q)\): Placement from parent to child frame
- Bias acceleration \(c(q,\dot{q})\): Often written as \(\dot{S}\dot{q}\)
Step 1: Mathematical Foundation
What You'll Do: Derive the mathematical equations that govern your joint's behavior. You will compute the spatial transform \(M(q)\), motion subspace \(S(q)\), and bias acceleration \(c(q,\dot{q})\) 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 \(q = [q_0, q_1, \ldots, q_{n-1}]\)?
- What is the relationship between \(q\) and \(\dot{q}\) (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:
\[{^p}X_c(q) = \begin{bmatrix} R(q) & 0 \\ -R(q)p(q)^\times & R(q) \end{bmatrix}
\]
Where:
- \(R(q) \in SO(3)\) is the rotation matrix
- \(p(q) \in \mathbb{R}^3\) is the translation vector
- \(p^\times\) denotes the skew-symmetric matrix
1.3 Derive the Motion Subspace
The motion subspace is the Jacobian of the spatial velocity with respect to joint velocity:
\[S = \frac{\partial {^p}v_J}{\partial \dot{q}}
\]
Where the spatial velocity in Pinocchio's convention is:
\[{^p}v_J = \begin{bmatrix} \dot{p}(q,\dot{q}) \\ \omega(q,\dot{q}) \end{bmatrix}
\]
- Note
- Pinocchio uses the convention \([v, \; \omega]^\top\) (linear, angular), while some literature (in the Featherstone book) uses \([\omega, \; v + p \times \omega]^\top\) (angular, linear). Be consistent with Pinocchio's convention.
Computing \(\omega\) (Angular Velocity)
For rotation-based joints, extract angular velocity from:
\[\omega = \text{axial}(\dot{R}(q)R(q)^T)
\]
Where \(\text{axial}\) extracts the vector from a skew-symmetric matrix.
Computing \(\dot{p}\) (Linear Velocity)
Differentiate the translation with respect to time:
\[\dot{p} = \frac{dp}{dt} = \frac{\partial p}{\partial q}\dot{q}
\]
1.4 Derive the Bias Acceleration
The bias acceleration (also called velocity product) is:
\[c = \dot{S}\dot{q}
\]
Where:
\[\dot{S} = \sum_{i=0}^{n_v-1} \frac{\partial S}{\partial q_i}\dot{q}_i
\]
Or equivalently:
\[\dot{S} = \frac{\partial^2 {^p}v_J}{\partial q \partial \dot{q}} \dot{q}
\]
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.
Files to Create/Modify:
2.1 File Structure
You will create the main joint header at this exact path:
pinocchio/include/pinocchio/multibody/joint/joint-<name>.hpp
You will also update these existing files:
- pinocchio/include/pinocchio/multibody/joint/fwd.hpp
- pinocchio/include/pinocchio/multibody/joint/joints.hpp
- pinocchio/include/pinocchio/multibody/joint/joint-collection.hpp
2.2 Define the Traits Struct
In joint-<name>.hpp, define the traits for your joint:
template<typename _Scalar, int _Options>
struct traits<JointModelMyJointTpl<_Scalar, _Options>>
{
enum {
NQ = 3,
NV = 3,
NVExtended = 3
};
typedef _Scalar Scalar;
typedef JointDataMyJointTpl<Scalar, Options> JointDataDerived;
typedef JointModelMyJointTpl<Scalar, Options> JointModelDerived;
typedef JointMotionSubspaceTpl<NV, Scalar, Options, NVExtended> Constraint_t;
typedef SE3Tpl<Scalar, Options> Transformation_t;
typedef MotionTpl<Scalar, Options> Motion_t;
typedef MotionTpl<Scalar, Options> Bias_t;
typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
};
- Important
- 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 3Section 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>>
{
typedef JointMyJointTpl<Scalar, Options> JointDerived;
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR;
ConfigVector_t joint_q;
TangentVector_t joint_v;
Constraint_t S;
Transformation_t M;
Motion_t v;
Bias_t c;
U_t U;
Dinv_t Dinv;
UD_t UDinv;
JointDataMyJointTpl()
: joint_q(ConfigVector_t::Zero())
, joint_v(TangentVector_t::Zero())
, S()
, M(Transformation_t::Identity())
, v(Motion_t::Zero())
, c(Bias_t::Zero())
, U()
, Dinv()
, UDinv()
{}
static std::string classname() { return "JointDataMyJoint"; }
std::string shortname() const { return classname(); }
};
2.4 Define the JointModel Struct
JointModel defines the joint's kinematics and dynamics:
template<typename _Scalar, int _Options>
struct JointModelMyJointTpl : public JointModelBase<JointModelMyJointTpl<_Scalar, _Options>>
{
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
typedef JointDataMyJointTpl<Scalar, Options> JointDataDerived;
using Base::id;
using Base::idx_q;
using Base::idx_v;
using Base::idx_vExtended;
using Base::setIndexes;
Scalar param_a;
Scalar param_b;
JointModelMyJointTpl()
: param_a(Scalar(1.0))
, param_b(Scalar(1.0))
{}
JointModelMyJointTpl(const Scalar & a, const Scalar & b)
: param_a(a)
, param_b(b)
{}
JointDataDerived createData() const
{
return JointDataDerived();
}
const std::vector<bool> hasConfigurationLimit() const
{
return {true, true, true};
}
const std::vector<bool> hasConfigurationLimitInTangent() const
{
return {true, true, true};
}
template<typename ConfigVector>
void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const;
\\TODO
template<typename ConfigVector, typename TangentVector>
void calc(
JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs,
const typename Eigen::MatrixBase<TangentVector> & vs) const;
\\TODO
template<typename VectorLike, typename Matrix6Like>
void calc_aba(
JointDataDerived & data,
const Eigen::MatrixBase<VectorLike> & armature,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I) const;
\\TODO
static std::string classname() { return "JointModelMyJoint"; }
std::string shortname() const { return classname(); }
};
2.5 Implement Forward Kinematics
calc(data, q): Compute M and S
template<typename ConfigVector>
void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
{
data.joint_q = qs.template segment<NQ>(idx_q());
Scalar c0, s0;
SINCOS(data.joint_q(0), &s0, &c0);
Scalar c1, s1;
SINCOS(data.joint_q(1), &s1, &c1);
computeSpatialTransform(c0, s0, c1, s1, , data);
computeMotionSubspace(c0, s0, c1, s1, , data);
}
calc(data, q, v): Compute M, S, v, and c
template<typename ConfigVector, typename TangentVector>
void calc(
JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs,
const typename Eigen::MatrixBase<TangentVector> & vs) const
{
calc(data, qs);
data.joint_v = vs.template segment<NV>(idx_v());
data.v.toVector().noalias() = data.S.matrix() * data.joint_v;
computeBias(, data);
}
2.6 Implement ABA Support
For the Articulated Body Algorithm:
template<typename VectorLike, typename Matrix6Like>
void calc_aba(
JointDataDerived & data,
const Eigen::MatrixBase<VectorLike> & armature,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I) const
{
data.U.noalias() = I * data.S.matrix();
data.StU.noalias() = data.S.transpose() * data.U;
data.StU.diagonal() += armature;
internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
}
2.7 Register the Joint
Forward Declaration
In include/pinocchio/multibody/joint/fwd.hpp:
template<typename Scalar, int Options = 0>
struct JointModelMyJointTpl;
typedef JointModelMyJointTpl<double> JointModelMyJoint;
Include in Joint Collection
In include/pinocchio/multibody/joint/joints.hpp:
#include "pinocchio/multibody/joint/joint-myjoint.hpp"
In include/pinocchio/multibody/joint/joint-collection.hpp:
typedef boost::variant<
JointModelMyJoint,
> JointModelVariant;
typedef boost::variant<
JointDataMyJoint,
> JointDataVariant;
2.8 Add Python Bindings
In bindings/python/multibody/joint/joints-models.hpp:
bp::class_<JointModelMyJoint>(
"JointModelMyJoint",
"My custom joint model",
bp::no_init)
.def(JointModelMyJointPythonVisitor<JointModelMyJoint>());
In bindings/python/multibody/joint/joints-datas.hpp, add similar binding for JointDataMyJoint.
Step 3: Motion Subspace Specialization
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 \(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: \(S = [0, 0, 0, 0, 0, 1]^T \) (single non-zero entry)
- Prismatic joint: \(S = [1, 0, 0, 0, 0, 0]^T \)
- 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:
template<typename Scalar, int Options>
struct JointMotionSubspaceMyJoint : JointMotionSubspaceBase<JointMotionSubspaceMyJoint<Scalar, Options>>
{
enum { NV = 3 };
typedef Eigen::Matrix<Scalar, 6, NV, Options> DenseBase;
typedef Eigen::Matrix<Scalar, NV, NV, Options> ReducedSquaredMatrix;
Eigen::Matrix<Scalar, 3, 1> axis;
DenseBase matrix() const
{
DenseBase S;
S.setZero();
return S;
}
template<typename MotionDerived>
typename MotionDerived::MotionPlain se3Action(const SE3Tpl<Scalar, Options> & m) const
{
}
template<typename MotionDerived>
typename MotionDerived::MotionPlain se3ActionInverse(const SE3Tpl<Scalar, Options> & m) const
{
}
template<typename D>
typename Motion::MotionPlain operator*(const Eigen::MatrixBase<D> & v) const
{
}
};
Then update your joint's traits to use this custom constraint:
typedef JointMotionSubspaceMyJoint<Scalar, Options> Constraint_t;
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.
Files to Modify:
- include/pinocchio/parsers/graph/joints.hpp (define graph joint)
- src/parsers/graph/model-graph-algo.cpp (create & add visitors)
- src/parsers/graph/model-graph.cpp (reversal visitors, if applicable)
- include/pinocchio/parsers/graph/graph-visitor.hpp (pose update, if applicable)
- include/pinocchio/parsers/graph/model-configuration-converter.hxx (converters, if reversible)
- bindings/python/parsers/graph/expose-edges.cpp (Python exposure)
Goal: After this step, users can create your joint from Python and use it in graph-based model construction.
4.1 Why ModelGraph?
The ModelGraph parser:
- Builds kinematic trees from high-level descriptions (URDF, SDF)
- Handles joint reversals (when kinematic tree orientation differs from description)
- Converts between different parameterizations
4.2 Define the Graph Joint Struct
Location: pinocchio/include/pinocchio/parsers/graph/joints.hpp
Add your joint struct to this file:
struct JointMyJoint
{
double param_a = 1.0;
double param_b = 1.0;
static constexpr int nq = 3;
static constexpr int nv = 3;
JointMyJoint() = default;
JointMyJoint(const double a, const double b)
: param_a(a)
, param_b(b)
{}
bool operator==(const JointMyJoint & other) const
{
return param_a == other.param_a && param_b == other.param_b;
}
};
Add it to the JointVariant typedef in the same file:
typedef boost::variant<
JointMyJoint,
> JointVariant;
4.3 Implement CreateJointModelVisitor
Location: src/parsers/graph/model-graph-algo.cpp
Add a visitor in the CreateJointModelVisitor class to convert your graph joint to a model joint:
ReturnType operator()(const JointMyJoint & joint) const
{
return JointModelMyJoint(joint.param_a, joint.param_b);
}
4.4 Implement AddJointModelVisitor
Location: Same file: src/parsers/graph/model-graph-algo.cpp If your joint cannot be reversed, you need to explicitly handle this:
void operator()(const JointMyJoint & joint, const BodyFrame & b_f)
{
if (!edge.forward)
PINOCCHIO_THROW_PRETTY(
std::invalid_argument,
"Graph - JointMyJoint cannot be reversed.");
addJointBetweenBodies(joint, b_f);
}
4.5 Support Reversible Joints (Advanced)
If your joint can be reversed, implement the following additional visitors.
ReverseJointGraphVisitor
Location: src/parsers/graph/model-graph.cpp
In src/parsers/graph/model-graph.cpp:
ReturnType operator()(const JointMyJoint & joint) const
{
return {JointMyJoint(joint.param_a, joint.param_b), SE3::Identity()};
}
UpdateJointGraphReversePoseVisitor
Location: include/pinocchio/parsers/graph/graph-visitor.hpp
In include/pinocchio/parsers/graph/graph-visitor.hpp:
SE3 operator()(const JointMyJoint & joint) const
{
return joint_calc(JointModelMyJoint(joint.param_a, joint.param_b));
}
Configuration and Tangent Converters
Location: include/pinocchio/parsers/graph/model-configuration-converter.hxx
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).
ConfigurationConverterVisitor:
ReturnType operator()(const JointModelMyJointTpl<Scalar, Options> &) const
{
if (joint.same_direction) {
q_target.template segment<JointModel::NQ>(offset_target) =
q_source.template segment<JointModel::NQ>(offset_source);
} else
{
q_target.template segment<JointModel::NQ>(offset_target) =
-q_source.template segment<JointModel::NQ>(offset_source);
}
}
TangentConverterVisitor:
ReturnType operator()(const JointModelMyJointTpl<Scalar, Options> &) const
{
if (joint.same_direction) {
v_target.template segment<JointModel::NV>(offset_target) =
v_source.template segment<JointModel::NV>(offset_source);
} else {
v_target.template segment<JointModel::NV>(offset_target)
-v_source.template segment<JointModel::NV>(offset_source);
}
}
- Warning
- 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 \(q\).
4.6 Add Python Exposure
Location: bindings/python/parsers/graph/expose-edges.cpp
Expose your graph joint to Python so it can be used from the Python API:
bp::class_<JointMyJoint>("JointMyJoint", bp::init<>())
.def(bp::init<double, double>())
.def_readwrite("param_a", &JointMyJoint::param_a)
.def_readwrite("param_b", &JointMyJoint::param_b);
Step 5: Testing
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-multibody-joint.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, \(S\), \(\dot{S}\), 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
This is your primary test file for the joint:
#include <boost/test/unit_test.hpp>
#include <pinocchio/multibody/joint/joint-<name>.hpp>
BOOST_AUTO_TEST_SUITE(JointMyJoint)
BOOST_AUTO_TEST_CASE(basic_kinematics)
{
typedef SE3::Vector3 Vector3;
JointModelMyJoint jmodel(1.0, 2.0);
JointDataMyJoint jdata = jmodel.createData();
Eigen::VectorXd q = Eigen::VectorXd::Random(3);
Eigen::VectorXd v = Eigen::VectorXd::Random(3);
jmodel.calc(jdata, q, v);
Motion v_expected;
v_expected.toVector().noalias() = jdata.S.matrix() * jdata.joint_v;
BOOST_CHECK(jdata.v.isApprox(v_expected));
}
BOOST_AUTO_TEST_CASE(test_motion_subspace_derivative)
{
JointModelMyJoint jmodel(1.0, 2.0);
JointDataMyJoint jdata = jmodel.createData();
Eigen::VectorXd q = Eigen::VectorXd::Random(3);
Eigen::VectorXd v = Eigen::VectorXd::Random(3);
jmodel.calc(jdata, q, v);
Motion c_analytic = jdata.c;
Eigen::Matrix<double, 6, 3> Sdot_fd = finiteDiffSdot(jmodel, jdata, q, v);
Motion c_fd;
c_fd.toVector().noalias() = Sdot_fd * jdata.joint_v;
BOOST_CHECK(c_analytic.isApprox(c_fd, 1e-6));
}
BOOST_AUTO_TEST_CASE(test_vs_composite)
{
}
BOOST_AUTO_TEST_SUITE_END()
Main pinocchio namespace.
5.2 Add to CMake
Location: unittest/CMakeLists.txt
Register your test so it gets built and run:
ADD_PINOCCHIO_UNIT_TEST(joint-myjoint)
5.3 Test Model Graph Integration
Location: unittest/model-graph.cpp
Add a test case for your joint in the graph tests:
BOOST_AUTO_TEST_CASE(test_graph_with_myjoint)
{
Graph graph;
Vertex v0 = graph.addVertex("world", BodyFrame::WORLD());
Vertex v1 = graph.addVertex("link1", BodyFrame(...));
JointMyJoint joint(1.0, 2.0);
graph.addEdge(v0, v1, joint, SE3::Identity(), JointLimits(...));
Model model = graph.buildModel();
BOOST_CHECK_EQUAL(model.njoints, 2);
BOOST_CHECK_EQUAL(model.nq, 3);
BOOST_CHECK_EQUAL(model.nv, 3);
}
5.4 Test Configuration Converters
Location: unittest/model-configuration-converter.cpp
(Only if your joint is reversible):
BOOST_AUTO_TEST_CASE(test_myjoint_converter)
{
Model model;
JointIndex idx = model.addJoint(0, JointModelMyJoint(1.0, 2.0), SE3::Identity(), "myjoint");
ModelConfigurationConverter converter(model, model_reversed);
Eigen::VectorXd q_converted(model_reversed.nq);
converter.convert(q, q_converted);
}
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
5.5 Test Serialization
Location: unittest/serialization-multibody-joint.cpp
Ensure your joint can be serialized and deserialized:
BOOST_AUTO_TEST_CASE(myjoint_serialization)
{
JointModelMyJoint jmodel(1.0, 2.0);
std::stringstream ss;
{
boost::archive::text_oarchive oa(ss);
oa << jmodel;
}
JointModelMyJoint jmodel_loaded;
{
boost::archive::text_iarchive ia(ss);
ia >> jmodel_loaded;
}
BOOST_CHECK(jmodel.param_a == jmodel_loaded.param_a);
BOOST_CHECK(jmodel.param_b == jmodel_loaded.param_b);
}
Summary Checklist
Use this checklist to track your implementation progress:
Mathematical Foundation
- Define configuration space (nq) and tangent space (nv)
- Derive spatial transform \(M(q)\)
- Derive motion subspace \(S(q)\)
- Derive bias acceleration \(c(q,\dot{q})\) or \(\dot{S}(q,\dot{q})\)
- Verify equations symbolically (SymPy, Mathematica)
Core Implementation
- Create include/pinocchio/multibody/joint/joint-<name>.hpp
- Define traits struct with NQ, NV, typedefs
- Implement JointDataMyJointTpl struct
- Implement JointModelMyJointTpl struct
- Implement calc(data, q) method
- Implement calc(data, q, v) method
- Implement calc_aba(...) method
- Add forward declaration in fwd.hpp
- Add Python bindings for JointModel and JointData
Motion Subspace Specialization (Optional)
- Determine if specialization is beneficial
- Implement custom constraint class
- Update traits to use custom constraint
- Implement spatial algebra operations
Model Graph Integration
- Add to JointVariant typedef
- Implement CreateJointModelVisitor in model-graph-algo.cpp
- Implement AddJointModelVisitor in model-graph-algo.cpp
- (Optional) Implement ReverseJointGraphVisitor in model-graph.cpp
- (Optional) Implement ConfigurationConverterVisitor in model-configuration-converter.hxx
- (Optional) Implement TangentConverterVisitor in model-configuration-converter.hxx
- Add Python bindings for graph joint in expose-edges.cpp
Testing
- Create unittest/joint-<name>.cpp
- Test basic kinematics (M, S, v)
- Test motion subspace derivative (Sdot, c) via finite differences
- Test dynamics (RNEA, ABA)
- Test equivalence with composite joints (if applicable)
- Update unittest/model-graph.cpp with graph tests
- Update unittest/model-configuration-converter.cpp with converter tests
- Update unittest/serialization-multibody-joint.cpp with serialization tests
- Add joint to unittest/all-joints.cpp
- Add joint to unittest/joint-generic.cpp
- Add joint to unittest/finite-differences.cpp
- Update unittest/CMakeLists.txt
Documentation
- Add docstrings to public methods
- Document joint parameters and their meanings
- Add example usage in examples/
- Update documentation in doc/
Additional Resources
Example: Quick Reference for Ellipsoid Joint
As a concrete example, the Ellipsoid joint implementation demonstrates:
- Configuration: \((q_0, q_1, q_2)\) - three Euler angles (XYZ)
- Parameters: \((a, b, c)\) - ellipsoid semi-axes
- Motion subspace: Dense 6×3 matrix (uses JointMotionSubspaceTpl)
- Reversal: Not supported (motion subspace is frame-dependent)
- Testing: Compared against SphericalZYX for rotation equivalence
Key files to review:
Happy implementing! If you have questions, consult the Pinocchio community or review existing joint implementations for guidance.
1.17.0