diff --git a/examples/inverse-dynamics.cpp b/examples/inverse-dynamics.cpp new file mode 100644 index 0000000000..4e020fd55c --- /dev/null +++ b/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 + +#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; +} 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))