pinocchio

Computes forward kinematics and Jacobians for articulated systems using Roy Featherstone's spatial algebra framework, which represents rigid body transformations as 6D spatial vectors (3D linear + 3D angular). The library uses template-based C++ to instantiate algorithms for different scalar types (double, float, CppAD, CasADi), enabling both numerical and symbolic computation paths. Spatial algebra operations are optimized through specialized matrix representations (6x6 spatial inertia matrices, 6D twists/wrenches) that reduce computational overhead compared to naive 4x4 homogeneous transformation approaches.

Computes joint torques required to achieve desired accelerations using the Recursive Newton-Euler algorithm (RNEA), then derives analytical gradients of torques with respect to configuration, velocity, and acceleration via automatic differentiation backends (CppAD, CasADi). The RNEA algorithm propagates spatial velocities and accelerations forward through the kinematic chain, then back-propagates forces and torques, achieving O(n) complexity. Derivatives are computed by either CppAD's tape-based AD or CasADi's symbolic graph construction, enabling gradient-based control optimization without finite differences.

Integrates with MeshCat (web-based 3D visualization) and Gepetto-Viewer (standalone viewer) to display robot configurations, trajectories, and collision geometries. The visualization system loads robot meshes from URDF, updates frame positions via forward kinematics, and renders trajectories as animated sequences. Users can interactively manipulate joint sliders to explore configurations or record videos of simulated motions. The system supports both C++ and Python interfaces, with Python providing more convenient high-level APIs.

Provides an abstract Joint interface that supports standard joint types (revolute, prismatic, spherical, planar, free-flyer) and enables users to define custom joint types via template specialization. Each joint type encapsulates its kinematics (forward kinematics, Jacobian computation) and dynamics (inertia propagation, force/torque mapping). The system uses a visitor pattern to dispatch operations to the appropriate joint type without runtime type checking. Custom joints can be added by implementing the Joint interface and registering with the Model.

Manages a hierarchical frame system where each frame is associated with a joint or link in the kinematic tree. Frames represent reference points (e.g., end-effector, sensor, contact point) and are updated via forward kinematics. The system supports both fixed frames (rigidly attached to links) and moving frames (attached to joints). Users can query frame positions, orientations, Jacobians, and velocities by frame name or ID. The frame system enables intuitive robot programming without explicit kinematic chain manipulation.

Computes analytical Jacobians and higher-order derivatives of kinematics and dynamics algorithms (forward kinematics, inverse dynamics, forward dynamics) via automatic differentiation or symbolic computation. Derivatives are computed by instantiating algorithms with CppAD or CasADi scalar types, which record/build expression graphs during evaluation. The system supports both first-order derivatives (Jacobians) and higher-order derivatives (Hessians, etc.) for optimization. Derivatives can be computed for entire algorithms or selected subsets via tape/graph slicing.

Solves the forward dynamics problem (computing accelerations from torques) using the Articulated Body Algorithm (ABA) with optional Cholesky decomposition for efficient constraint handling. For unconstrained systems, ABA runs in O(n) time by computing spatial inertias and accelerations recursively. For constrained systems (contact, joint limits), Pinocchio uses Contact Cholesky decomposition to factorize the constraint Jacobian and solve for contact forces, then computes accelerations. The Delassus operator (contact constraint Hessian) is computed via the Cholesky factor, enabling proximal solvers for friction cones and inequality constraints.

Parses robot models from URDF, SDF, and MJCF file formats into Pinocchio's internal Model representation, handling joint hierarchies, inertial properties, collision/visual geometries, and joint constraints. The parser uses XML parsing (via tinyxml2 or urdfdom) to extract kinematic chains, then constructs a tree structure with support for mimic joints (dependent joints that follow a linear relationship to parent joints). Mimic joints are handled via an extended Model class that tracks joint dependencies, enabling efficient computation of dependent joint accelerations and forces without explicit constraint solving.

Manages collision geometries (meshes, primitives) associated with robot frames and provides collision detection via integration with the hpp-fcl library. Geometries are stored in a GeometryModel that parallels the kinematic Model, with each geometry linked to a frame and updated via forward kinematics. Collision queries (distance, penetration depth, contact points) are computed by transforming geometries to world coordinates and invoking FCL's BVH-based algorithms. The system supports both collision checking (binary yes/no) and distance computation for safety margins.

Provides Python bindings for core Pinocchio functionality via pybind11, enabling NumPy array interoperability for kinematics, dynamics, and collision queries. Bindings are generated via template specialization for common scalar types (double, float) and expose high-level APIs (RobotWrapper, Data structures) alongside low-level algorithm functions. NumPy arrays are automatically converted to/from Eigen vectors and matrices, eliminating data copy overhead for large arrays. The Python interface supports both eager evaluation (immediate computation) and lazy evaluation (symbolic graphs via CasADi).

Enables analytical differentiation of kinematics and dynamics algorithms by instantiating Pinocchio templates with CppAD (tape-based AD) or CasADi (symbolic graph) scalar types instead of double/float. CppAD records operations into a tape during forward evaluation, then replays the tape backward to compute derivatives; CasADi builds a symbolic expression graph that can be compiled to C code or used for further symbolic manipulation. Users can switch backends by changing template parameters without modifying algorithm code. Derivatives are computed for entire algorithms (RNEA, ABA, forward kinematics) or selected subsets via tape/graph slicing.

Computes the center of mass (CoM) position and velocity, as well as linear and angular momentum of the entire robot or selected subtrees. The computation traverses the kinematic tree, accumulating mass-weighted positions and velocities from each link. Momentum Jacobians (derivatives of momentum w.r.t. joint velocities) are computed analytically, enabling momentum-based control and balance optimization. The system supports both numerical computation and analytical differentiation via CppAD/CasADi for gradient-based balance control.

Enables Pinocchio algorithms to work with arbitrary scalar types (double, float, long double, CppAD, CasADi, Boost multiprecision) via C++ template specialization. The library instantiates core algorithms for common types at compile time, and users can extend with custom types by specializing template functions. This design allows the same algorithm code to run with different precision levels or symbolic types without modification. Multiprecision support is useful for high-precision trajectory optimization or sensitivity analysis where numerical errors accumulate.

Provides a high-level Python/C++ API (RobotWrapper class) that encapsulates Model, Data, and GeometryModel objects, offering convenience methods for common tasks (forward kinematics, inverse kinematics, collision checking, visualization). RobotWrapper automatically manages Data structure allocation and updates, reducing boilerplate code. It also provides utility functions for loading robots from URDF, setting up visualizers, and accessing frame/joint information by name. The wrapper is designed for rapid prototyping and interactive use, trading some performance for ease of use.