feat(ik): added pinocchio ik

Author: juelg

python/rcsss/_core/common.pyi

@@ -19,6 +19,7 @@ __all__ = [
     "IdentityRotQuartVec",
     "IdentityTranslation",
     "NRobotsWithGripper",
+    "Pin",
     "Pose",
     "RConfig",
     "RL",
@@ -69,6 +70,9 @@ class NRobotsWithGripper:
     ) -> None: ...
     def shut(self, idxs: list[int]) -> None: ...
 
+class Pin(IK):
+    def __init__(self, urdf_path: str) -> None: ...
+
 class Pose:
     def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
     @typing.overload

src/common/CMakeLists.txt

@@ -1,3 +1,3 @@
 add_library(common)
 target_sources(common PRIVATE Pose.cpp NRobotsWithGripper.cpp Robot.cpp IK.cpp)
-target_link_libraries(common PUBLIC Eigen3::Eigen mdl)
+target_link_libraries(common PUBLIC Eigen3::Eigen mdl pinocchio::pinocchio)

src/common/IK.cpp

@@ -12,6 +12,12 @@
 #include <rl/mdl/Kinematic.h>
 #include <rl/mdl/UrdfFactory.h>
 
+#include <pinocchio/parsers/urdf.hpp>
+
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+
 namespace rcs {
 namespace common {
 
@@ -21,8 +27,8 @@ RL::RL(const std::string& urdf_path, size_t max_duration_ms) : rl_data() {
       std::dynamic_pointer_cast<rl::mdl::Kinematic>(this->rl_data.mdl);
   this->rl_data.ik = std::make_shared<rl::mdl::JacobianInverseKinematics>(
       this->rl_data.kin.get());
-  this->rl_data.ik->setRandomRestarts(0);
-  this->rl_data.ik->setEpsilon(1e-3);
+  this->rl_data.ik->setRandomRestarts(this->random_restarts);
+  this->rl_data.ik->setEpsilon(this->eps);
   this->rl_data.ik->setDuration(std::chrono::milliseconds(max_duration_ms));
 }
 
@@ -45,5 +51,56 @@ std::optional<Vector7d> RL::ik(const Pose& pose, const Vector7d& q0,
   }
 }
 
+Pin::Pin(const std::string& urdf_path) : model() {
+  pinocchio::urdf::buildModel(urdf_path, this->model);
+  this->data = pinocchio::Data(this->model);
+}
+
+std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
+                                const Pose& tcp_offset) {
+  rcs::common::Pose new_pose = pose * tcp_offset.inverse();
+  Vector7d q(q0);
+  const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation());
+  pinocchio::Data::Matrix6x J(6, model.nv);
+  J.setZero();
+  bool success = false;
+  Vector6d err;
+  Eigen::VectorXd v(model.nv);
+  for (int i = 0;; i++) {
+    pinocchio::forwardKinematics(model, data, q);
+    const pinocchio::SE3 iMd = data.oMi[this->JOINT_ID].actInv(oMdes);
+    err = pinocchio::log6(iMd).toVector();  // in joint frame
+    if (err.norm() < this->eps) {
+      success = true;
+      break;
+    }
+    if (i >= this->IT_MAX) {
+      success = false;
+      break;
+    }
+    pinocchio::computeJointJacobian(model, data, q, this->JOINT_ID,
+                                    J);  // J in joint frame
+    pinocchio::Data::Matrix6 Jlog;
+    pinocchio::Jlog6(iMd.inverse(), Jlog);
+    J = -Jlog * J;
+    pinocchio::Data::Matrix6 JJt;
+    JJt.noalias() = J * J.transpose();
+    JJt.diagonal().array() += this->damp;
+    v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
+    q = pinocchio::integrate(model, q, v * this->DT);
+    if (!(i % 10))
+      std::cout << i << ": error = " << err.transpose() << std::endl;
+  }
+  if (success) {
+    // print amount of iterations
+    std::cout << "IK success after " << std::to_string(IT_MAX) << " iterations"
+              << std::endl;
+    return q;
+  } else {
+    std::cout << "IK failed" << std::endl;
+    return std::nullopt;
+  }
+}
+
 }  // namespace common
 }  // namespace rcs

src/common/IK.h

@@ -8,8 +8,12 @@
 #include <eigen3/Eigen/Geometry>
 #include <memory>
 #include <optional>
+#include <pinocchio/parsers/urdf.hpp>
 
 #include "Pose.h"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
 #include "utils.h"
 
 namespace rcs {
@@ -25,16 +29,34 @@ class IK {
 
 class RL : public IK {
  private:
-  struct RLData {
+  const int random_restarts = 0;
+  const double eps = 1e-3;
+  struct {
     std::shared_ptr<rl::mdl::Model> mdl;
     std::shared_ptr<rl::mdl::Kinematic> kin;
     std::shared_ptr<rl::mdl::JacobianInverseKinematics> ik;
-  };
-  RLData rl_data;
+  } rl_data;
 
  public:
   RL(const std::string& urdf_path, size_t max_duration_ms = 300);
-  ~RL() override = default;
+  std::optional<Vector7d> ik(
+      const Pose& pose, const Vector7d& q0,
+      const Pose& tcp_offset = Pose::Identity()) override;
+};
+
+class Pin : public IK {
+ private:
+  const double eps = 1e-4;
+  const int IT_MAX = 1000;
+  const double DT = 1e-1;
+  const double damp = 1e-6;
+  const int JOINT_ID = 7;
+
+  pinocchio::Model model;
+  pinocchio::Data data;
+
+ public:
+  Pin(const std::string& urdf_path);
   std::optional<Vector7d> ik(
       const Pose& pose, const Vector7d& q0,
       const Pose& tcp_offset = Pose::Identity()) override;

src/pybind/CMakeLists.txt

@@ -3,7 +3,7 @@ target_link_libraries(_core PRIVATE hw sim common)
 target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION})
 
 set_target_properties(_core PROPERTIES
-    INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco"
+    INSTALL_RPATH "$ORIGIN;$ORIGIN/../mujoco;$ORIGIN/../cmeel.prefix/lib"
     INTERPROCEDURAL_OPTIMIZATION TRUE
 )
 set_target_properties(franka PROPERTIES

src/pybind/rcsss.cpp

@@ -220,6 +220,10 @@ PYBIND11_MODULE(_core, m) {
       .def(py::init<const std::string &, size_t>(), py::arg("urdf_path"),
            py::arg("max_duration_ms") = 300);
 
+  py::class_<rcs::common::Pin, rcs::common::IK,
+             std::shared_ptr<rcs::common::Pin>>(common, "Pin")
+      .def(py::init<const std::string &>(), py::arg("urdf_path"));
+
   py::class_<rcs::common::RConfig>(common, "RConfig");
   py::class_<rcs::common::RState>(common, "RState");
   py::class_<rcs::common::GConfig>(common, "GConfig");