From c9a15aa33f32fbf6eb1dbc7c0f31c70527c506d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?St=C3=A9phane=20Caron?= Date: Mon, 13 Nov 2023 14:46:09 +0100 Subject: [PATCH 1/3] Add ID example --- examples/inverse-dynamics.cpp | 42 +++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 examples/inverse-dynamics.cpp diff --git a/examples/inverse-dynamics.cpp b/examples/inverse-dynamics.cpp new file mode 100644 index 0000000000..bc1f9f2e85 --- /dev/null +++ b/examples/inverse-dynamics.cpp @@ -0,0 +1,42 @@ +#include + +#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 pass it as a 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 Nm) + std::cout << "Joint torques: " << data.tau.transpose() << std::endl; + return 0; +} From b482ee4ea664eefb1d4cb0a41b3ccd0e98f9537e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?St=C3=A9phane=20Caron?= Date: Mon, 13 Nov 2023 14:54:46 +0100 Subject: [PATCH 2/3] 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 #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)) From 0952134c895876bdab6b488ec6c924630e9816f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?St=C3=A9phane=20Caron?= Date: Mon, 13 Nov 2023 14:56:24 +0100 Subject: [PATCH 3/3] Make the two examples closer to each other --- examples/inverse-dynamics.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/examples/inverse-dynamics.cpp b/examples/inverse-dynamics.cpp index 381bbd6216..4e020fd55c 100644 --- a/examples/inverse-dynamics.cpp +++ b/examples/inverse-dynamics.cpp @@ -1,6 +1,11 @@ // 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 #include "pinocchio/algorithm/joint-configuration.hpp"