Add inverse dynamics examples

examples/inverse-dynamics.cpp

@@ -0,0 +1,50 @@
+// 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.
+ */
+
+#include <iostream>
+
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+// PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own
+// directory here.
+#ifndef PINOCCHIO_MODEL_DIR
+#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
+#endif
+
+int main(int argc, char** argv) {
+  using namespace pinocchio;
+
+  // 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(
+                                      "/example-robot-data/robots/"
+                                      "ur_description/urdf/ur5_robot.urdf")
+          : argv[1];
+
+  // Load the URDF model
+  Model model;
+  pinocchio::urdf::buildModel(urdf_filename, model);
+
+  // Build a data frame associated with the model
+  Data data(model);
+
+  // Sample a random joint configuration, joint velocities and accelerations
+  Eigen::VectorXd q = randomConfiguration(model);       // in rad for the UR5
+  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);  // in rad/s for the UR5
+  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);  // in rad/s² for the UR5
+
+  // 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 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))