Merge pull request #2428 from jcarpent/topic/examples

Add inverse-kinematics for translation error only

Author: jcarpent

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))

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(

examples/inverse-kinematics-3d.cpp

@@ -0,0 +1,68 @@
+#include <iostream>
+
+#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;
+}

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)