Workshop 4: Inverse Dynamics

Inverse dynamics with Pinnochio Library

We will use the Pinnochio Library, a dynamics library that provide methods to calculate inverse dynamics of manipulators from URDF files. The library is already installed in the container. Here are instructions on how to create a ros package containing a node that calculates the inverse dynamic

Creating the package

First, navigate to your workspace (/src) and create a new Python package.

ros2 pkg create --build-type ament_python pinocchio_dynamics --dependencies rclpy

Create the Inverse Dynamics Nde

Create a file named dynamics_node.py inside pinocchio_dynamics/pinocchio_dynamics/.The goal here is to solve the general equation of motion for inverse dynamics:

$$\tau = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q)$$

Where $\tau$ represents the joint torques required to achieve a specific acceleration $\ddot{q}$.

import rclpy
from rclpy.node import Node
import pinocchio as pin
import numpy as np
import os
from ament_index_python.packages import get_package_share_directory

class PinocchioDynamicsNode(Node):
    def __init__(self):
        super().__init__('pinocchio_dynamics_node')
        
        # 1. Locate and Load the URDF
        # Replace 'your_description_package' and 'robot.urdf' with your actual files
        try:
            package_name = 'your_description_package'
            urdf_name = 'robot.urdf'
            urdf_path = os.path.join(get_package_share_directory(package_name), 'urdf', urdf_name)
        except Exception:
            self.get_logger().error("Could not find URDF path. Check package name.")
            return

        # 2. Build the Pinocchio Model
        self.model = pin.buildModelFromUrdf(urdf_path)
        self.data = self.model.createData()
        
        self.get_logger().info(f"Model loaded. Degrees of freedom: {self.model.nv}")

        # Create a timer to simulate a control loop
        self.timer = self.create_timer(0.1, self.calculate_inverse_dynamics)

    def calculate_inverse_dynamics(self):
        # 3. Define state: Position (q), Velocity (v), and Acceleration (a)
        # In a real scenario, these would come from /joint_states or a controller
        q = pin.neutral(self.model)
        v = np.zeros(self.model.nv)
        a = np.zeros(self.model.nv)

        # 4. Compute RNEA (Recursive Newton-Euler Algorithm)
        # This calculates the torques (tau)
        tau = pin.rnea(self.model, self.data, q, v, a)

        self.get_logger().info(f"Calculated Torques: {tau}")

def main(args=None):
    rclpy.init(args=args)
    node = PinocchioDynamicsNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Configuration and Build

You need to let ROS 2 know about your script and ensure the environment is ready.

Inside your setup.py, find the entry_points section and add your node:

entry_points={
    'console_scripts': [
        'dynamics_node = pinocchio_dynamics.dynamics_node:main'
    ],
},

Update package.xml. Ensure Pinocchio is listed as an exec dependency:

<exec_depend>pinocchio</exec_depend>

Build the Package, back in your workspace root:

colcon build --packages-select pinocchio_dynamics
source install/setup.bash

Further Notes

• If your URDF uses package:// paths for meshes, Pinocchio needs to know where those packages are. You can pass a list of paths to pin.buildModelFromUrdf(urdf_path, package_dirs).
• You can find the Pinocchio documentation here