fix: pinocchio ik to use custom frame

Author: juelg

CMakeLists.txt

@@ -49,7 +49,7 @@ find_package(Eigen3 REQUIRED)
 find_package(glfw3 REQUIRED)
 find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
 find_package(MuJoCo REQUIRED)
-
+find_package(pinocchio REQUIRED)
 FetchContent_Declare(
     libfranka
     GIT_REPOSITORY https://github.com/frankaemika/libfranka.git

python/rcsss/_core/common.pyi

@@ -71,7 +71,7 @@ class NRobotsWithGripper:
     def shut(self, idxs: list[int]) -> None: ...
 
 class Pin(IK):
-    def __init__(self, urdf_path: str) -> None: ...
+    def __init__(self, urdf_path: str, frame_id: str = "fr3_link8") -> None: ...
 
 class Pose:
     def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...

src/common/IK.cpp

@@ -12,12 +12,12 @@
 #include <rl/mdl/Kinematic.h>
 #include <rl/mdl/UrdfFactory.h>
 
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
 #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 {
 
@@ -51,9 +51,14 @@ std::optional<Vector7d> RL::ik(const Pose& pose, const Vector7d& q0,
   }
 }
 
-Pin::Pin(const std::string& urdf_path) : model() {
+Pin::Pin(const std::string& urdf_path, const std::string& frame_id) : model() {
   pinocchio::urdf::buildModel(urdf_path, this->model);
   this->data = pinocchio::Data(this->model);
+  this->FRAME_ID = model.getFrameId(frame_id);
+  if (FRAME_ID == -1) {
+    throw std::runtime_error(
+        frame_id + " frame id could not be found in the provided URDF");
+  }
 }
 
 std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
@@ -68,8 +73,9 @@ std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
   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
+    pinocchio::updateFramePlacements(model, data);
+    const pinocchio::SE3 iMd = data.oMf[this->FRAME_ID].actInv(oMdes);
+    err = pinocchio::log6(iMd).toVector();
     if (err.norm() < this->eps) {
       success = true;
       break;
@@ -78,8 +84,7 @@ std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
       success = false;
       break;
     }
-    pinocchio::computeJointJacobian(model, data, q, this->JOINT_ID,
-                                    J);  // J in joint frame
+    pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J);
     pinocchio::Data::Matrix6 Jlog;
     pinocchio::Jlog6(iMd.inverse(), Jlog);
     J = -Jlog * J;
@@ -88,16 +93,10 @@ std::optional<Vector7d> Pin::ik(const Pose& pose, const Vector7d& q0,
     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;
   }
 }

src/common/IK.h

@@ -9,6 +9,7 @@
 #include <memory>
 #include <optional>
 #include <pinocchio/parsers/urdf.hpp>
+#include <string>
 
 #include "Pose.h"
 #include "pinocchio/algorithm/jacobian.hpp"
@@ -50,13 +51,13 @@ class Pin : public IK {
   const int IT_MAX = 1000;
   const double DT = 1e-1;
   const double damp = 1e-6;
-  const int JOINT_ID = 7;
+  int FRAME_ID;
 
   pinocchio::Model model;
   pinocchio::Data data;
 
  public:
-  Pin(const std::string& urdf_path);
+  Pin(const std::string& urdf_path, const std::string& frame_id);
   std::optional<Vector7d> ik(
       const Pose& pose, const Vector7d& q0,
       const Pose& tcp_offset = Pose::Identity()) override;

src/hw/CMakeLists.txt

@@ -6,5 +6,5 @@ target_link_libraries(
   PRIVATE math
 )
 
-add_executable(test_hw main.cpp)
-target_link_libraries(test_hw PRIVATE hw)
+# add_executable(test_hw main.cpp)
+# target_link_libraries(test_hw PRIVATE hw)

src/pybind/rcsss.cpp

@@ -222,7 +222,8 @@ PYBIND11_MODULE(_core, m) {
 
   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"));
+      .def(py::init<const std::string &, const std::string &>(),
+           py::arg("urdf_path"), py::arg("frame_id") = "fr3_link8");
 
   py::class_<rcs::common::RConfig>(common, "RConfig");
   py::class_<rcs::common::RState>(common, "RState");

src/sim/CMakeLists.txt

@@ -4,5 +4,5 @@ target_sources(sim
 )
 target_link_libraries(sim PUBLIC glfw common MuJoCo::MuJoCo mujoco_ui_lib)
 
-add_executable(test_sim test.cpp sim.cpp)
-target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common glfw)
+# add_executable(test_sim test.cpp sim.cpp)
+# target_link_libraries(test_sim PRIVATE sim MuJoCo::MuJoCo common glfw pinocchio::pinocchio)

src/sim/test.cpp

@@ -15,7 +15,7 @@
 // const std::string mjcf = MODEL_DIR "/mjcf/fr3_modular/scene.xml";
 // const std::string urdf = MODEL_DIR "/fr3/urdf/fr3_from_panda.urdf";
 const std::string mjcf = "build/_deps/scenes-src/scenes/lab/scene.xml";
-const std::string urdf = "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf";
+const std::string urdf_path = "build/_deps/scenes-src/scenes/lab/assets/fr3.urdf";
 static const Eigen::Matrix<double, 1, 3, Eigen::RowMajor> iso_cube_center(
     0.498, 0.0, 0.226);
 static const float iso_cube_size = 0.4;
@@ -126,7 +126,7 @@ int test_sim() {
   sim->set_config(cfg);
   std::string id = "0";
 
-  auto ik = std::make_shared<rcs::common::RL>(urdf);
+  auto ik = std::make_shared<rcs::common::RL>(urdf_path);
   auto fr3 = rcs::sim::FR3(sim, id, ik);
   auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset());
   rcs::sim::FR3Config fr3_config = *fr3.get_parameters();