Add Python inverse-dynamics example

examples/inverse-dynamics.cpp

@@ -1,3 +1,6 @@
+// Copyright 2023 Inria
+// SPDX-License-Identifier: BSD-2-Clause
+
 #include <iostream>
 
 #include "pinocchio/algorithm/joint-configuration.hpp"
@@ -13,7 +16,7 @@
 int main(int argc, char** argv) {
   using namespace pinocchio;
 
-  // Change to your own URDF file here, or pass it as a command-line argument
+  // Change to your own URDF file here, or give a path as command-line argument
   const std::string urdf_filename =
       (argc <= 1)
           ? PINOCCHIO_MODEL_DIR + std::string(
@@ -36,7 +39,7 @@ int main(int argc, char** argv) {
   // Computes the inverse dynamics (RNEA) for all the joints of the robot
   Eigen::VectorXd tau = pinocchio::rnea(model, data, q, v, a);
 
-  // Print out to the vector of joint torques (in Nm)
+  // Print out to the vector of joint torques (in N.m)
   std::cout << "Joint torques: " << data.tau.transpose() << std::endl;
   return 0;
 }

examples/inverse-dynamics.py

@@ -0,0 +1,35 @@
+# Copyright 2023 Inria
+# SPDX-License-Identifier: BSD-2-Clause
+
+"""
+In this short script, we show how to compute inverse dynamics (RNEA), i.e. the
+vector of joint torques corresponding to a given motion.
+"""
+
+from os.path import abspath, dirname, join
+
+import numpy as np
+import pinocchio as pin
+
+# Load the model from a URDF file
+# Change to your own URDF file here, or give a path as command-line argument
+pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models/")
+model_path = join(pinocchio_model_dir, "example-robot-data/robots")
+mesh_dir = pinocchio_model_dir
+urdf_filename = "ur5_robot.urdf"
+urdf_model_path = join(join(model_path, "ur_description/urdf/"), urdf_filename)
+model, _, _ = pin.buildModelsFromUrdf(urdf_model_path, package_dirs=mesh_dir)
+
+# Build a data frame associated with the model
+data = model.createData()
+
+# Sample a random joint configuration, joint velocities and accelerations
+q = pin.randomConfiguration(model)  # in rad for the UR5
+v = np.random.rand(model.nv, 1)  # in rad/s for the UR5
+a = np.random.rand(model.nv, 1)  # in rad/s² for the UR5
+
+# Computes the inverse dynamics (RNEA) for all the joints of the robot
+tau = pin.rnea(model, data, q, v, a)
+
+# Print out to the vector of joint torques (in N.m)
+print("Joint torques: " + str(tau))