From 56f7c853bcd9536a489e53cdc36e3cdfe3c70a72 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?St=C3=A9phane=20Caron?= <stephane.caron@normalesup.org>
Date: Mon, 13 Nov 2023 14:54:46 +0100
Subject: [PATCH] Add Python inverse-dynamics example

---
 examples/inverse-dynamics.cpp |  7 +++++--
 examples/inverse-dynamics.py  | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+), 2 deletions(-)
 create mode 100644 examples/inverse-dynamics.py

diff --git a/examples/inverse-dynamics.cpp b/examples/inverse-dynamics.cpp
index bc1f9f2e85..381bbd6216 100644
--- a/examples/inverse-dynamics.cpp
+++ b/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;
 }
diff --git a/examples/inverse-dynamics.py b/examples/inverse-dynamics.py
new file mode 100644
index 0000000000..677128dcbf
--- /dev/null
+++ b/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))