From 9fdcfe4ac678cfe00d314cf8b8921e102f129a62 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 16:46:42 +0200 Subject: [PATCH 1/2] examples: add inverse-kinematics for translation error only --- examples/CMakeLists.txt | 8 ++-- examples/inverse-kinematics-3d.cpp | 68 ++++++++++++++++++++++++++++++ examples/inverse-kinematics-3d.py | 47 +++++++++++++++++++++ 3 files changed, 120 insertions(+), 3 deletions(-) create mode 100644 examples/inverse-kinematics-3d.cpp create mode 100644 examples/inverse-kinematics-3d.py diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index dc1629b45c..bbb2e5427a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (c) 2015-2022 CNRS INRIA +# Copyright (c) 2015-2024 CNRS INRIA # # Compute flags outside the macro to avoid recomputing it for each tests @@ -46,6 +46,7 @@ function(ADD_PINOCCHIO_CPP_EXAMPLE EXAMPLE) endfunction() add_pinocchio_cpp_example(inverse-kinematics) +add_pinocchio_cpp_example(inverse-kinematics-3d) add_pinocchio_cpp_example(overview-simple) add_pinocchio_cpp_example(overview-lie) add_pinocchio_cpp_example(overview-SE3) @@ -70,8 +71,9 @@ if(BUILD_WITH_COLLISION_SUPPORT) endif() if(BUILD_PYTHON_INTERFACE) - set(${PROJECT_NAME}_PYTHON_EXAMPLES inverse-kinematics overview-simple kinematics-derivatives - forward-dynamics-derivatives inverse-dynamics-derivatives) + set(${PROJECT_NAME}_PYTHON_EXAMPLES + inverse-kinematics inverse-kinematics-3d overview-simple kinematics-derivatives + forward-dynamics-derivatives inverse-dynamics-derivatives) if(BUILD_WITH_URDF_SUPPORT) list( diff --git a/examples/inverse-kinematics-3d.cpp b/examples/inverse-kinematics-3d.cpp new file mode 100644 index 0000000000..c1e19a8ab6 --- /dev/null +++ b/examples/inverse-kinematics-3d.cpp @@ -0,0 +1,68 @@ +#include + +#include "pinocchio/multibody/sample-models.hpp" +#include "pinocchio/spatial/explog.hpp" +#include "pinocchio/algorithm/kinematics.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" + +int main(int /* argc */, char ** /* argv */) +{ + pinocchio::Model model; + pinocchio::buildModels::manipulator(model); + pinocchio::Data data(model); + + const int JOINT_ID = 6; + const pinocchio::SE3 oMdes(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.)); + + Eigen::VectorXd q = pinocchio::neutral(model); + const double eps = 1e-4; + const int IT_MAX = 1000; + const double DT = 1e-1; + const double damp = 1e-12; + + pinocchio::Data::Matrix6x joint_jacobian(6, model.nv); + joint_jacobian.setZero(); + + bool success = false; + Eigen::Vector3d err; + Eigen::VectorXd v(model.nv); + for (int i = 0;; i++) + { + pinocchio::forwardKinematics(model, data, q); + const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes); + err = iMd.translation(); // in joint frame + if (err.norm() < eps) + { + success = true; + break; + } + if (i >= IT_MAX) + { + success = false; + break; + } + pinocchio::computeJointJacobian( + model, data, q, JOINT_ID, joint_jacobian); // joint_jacobian expressed in the joint frame + const auto J = -joint_jacobian.topRows<3>(); // Jacobian associated with the error + const Eigen::Matrix3d JJt = J * J.transpose() + damp * Eigen::Matrix3d::Identity(); + v.noalias() = -J.transpose() * JJt.ldlt().solve(err); + q = pinocchio::integrate(model, q, v * DT); + if (!(i % 10)) + std::cout << i << ": error = " << err.transpose() << std::endl; + } + + if (success) + { + std::cout << "Convergence achieved!" << std::endl; + } + else + { + std::cout + << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" + << std::endl; + } + + std::cout << "\nresult: " << q.transpose() << std::endl; + std::cout << "\nfinal error: " << err.transpose() << std::endl; +} diff --git a/examples/inverse-kinematics-3d.py b/examples/inverse-kinematics-3d.py new file mode 100644 index 0000000000..17563e122e --- /dev/null +++ b/examples/inverse-kinematics-3d.py @@ -0,0 +1,47 @@ +from __future__ import print_function + +import numpy as np +from numpy.linalg import norm, solve + +import pinocchio + +model = pinocchio.buildSampleModelManipulator() +data = model.createData() + +JOINT_ID = 6 +oMdes = pinocchio.SE3(np.eye(3), np.array([1.0, 0.0, 1.0])) + +q = pinocchio.neutral(model) +eps = 1e-4 +IT_MAX = 1000 +DT = 1e-1 +damp = 1e-12 + +it = 0 +while True: + pinocchio.forwardKinematics(model, data, q) + iMd = data.oMi[JOINT_ID].actInv(oMdes) + err = iMd.translation + if norm(err) < eps: + success = True + break + if it >= IT_MAX: + success = False + break + J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) # in joint frame + J = -J[:3, :] # linear part of the Jacobian + v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(3), err)) + q = pinocchio.integrate(model, q, v * DT) + if not it % 10: + print("%d: error = %s" % (it, err.T)) + it += 1 + +if success: + print("Convergence achieved!") +else: + print( + "\nWarning: the iterative algorithm has not reached convergence to the desired precision" + ) + +print("\nresult: %s" % q.flatten().tolist()) +print("\nfinal error: %s" % err.T) From c0284f39dfed0a8701dd88e438542025cd59e49c Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 16:58:02 +0200 Subject: [PATCH 2/2] changelog: sync --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 58cb1f7f91..7299be685e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Default visualizer can be changed with `PINOCCHIO_VIEWER` environment variable ([#2419](https://github.com/stack-of-tasks/pinocchio/pull/2419)) +- Add more Python and C++ examples related to inverse kinematics with 3d tasks ([#2428](https://github.com/stack-of-tasks/pinocchio/pull/2428)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400))