feat(pin ik): nullspace and joint limits

Author: juelg

include/rcs/Kinematics.h

@@ -38,8 +38,17 @@ class Pin : public Kinematics {
   pinocchio::Model model;
   pinocchio::Data data;
 
+  VectorXd q_lower;
+  VectorXd q_upper;
+  bool enforce_limits;
+
+  VectorXd nullspace_q;
+  double nullspace_gain;
+
  public:
-  Pin(const std::string& path, const std::string& frame_id, bool urdf);
+  Pin(const std::string& path, const std::string& frame_id, bool urdf,
+      const VectorXd& nullspace_q = VectorXd(), double nullspace_gain = 0.0,
+      bool enforce_limits = true);
   std::optional<VectorXd> inverse(
       const Pose& pose, const VectorXd& q0,
       const Pose& tcp_offset = Pose::Identity()) override;

python/rcs/_core/common.pyi

@@ -315,7 +315,15 @@ class RotVec:
     ) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
 
 class Pin(Kinematics):
-    def __init__(self, path: str, frame_id: str = "fr3_link8", urdf: bool = False) -> None: ...
+    def __init__(
+        self,
+        path: str,
+        frame_id: str = "fr3_link8",
+        urdf: bool = False,
+        nullspace_q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] = ...,
+        nullspace_gain: float = 0.0,
+        enforce_limits: bool = True,
+    ) -> None: ...
 
 def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
 def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...

src/pybind/rcs.cpp

@@ -378,9 +378,12 @@ PYBIND11_MODULE(_core, m) {
 
   py::class_<rcs::common::Pin, rcs::common::Kinematics,
              std::shared_ptr<rcs::common::Pin>>(common, "Pin")
-      .def(py::init<const std::string&, const std::string&, bool>(),
+      .def(py::init<const std::string&, const std::string&, bool,
+                    const rcs::common::VectorXd&, double, bool>(),
            py::arg("path"), py::arg("frame_id") = "fr3_link8",
-           py::arg("urdf") = false);
+           py::arg("urdf") = false,
+           py::arg("nullspace_q") = rcs::common::VectorXd(),
+           py::arg("nullspace_gain") = 0.0, py::arg("enforce_limits") = true);
 
   bind_type_class<rcs::common::RobotType>(common, "RobotType")
       .def_readonly_static("FR3", &rcs::common::RobotType::FR3)

src/rcs/Kinematics.cpp

@@ -10,8 +10,9 @@
 namespace rcs {
 namespace common {
 
-Pin::Pin(const std::string& path, const std::string& frame_id,
-         bool urdf = false)
+Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf,
+         const VectorXd& nullspace_q, double nullspace_gain,
+         bool enforce_limits)
     : model() {
   if (urdf) {
     pinocchio::urdf::buildModel(path, this->model);
@@ -24,6 +25,16 @@ Pin::Pin(const std::string& path, const std::string& frame_id,
     throw std::runtime_error(
         frame_id + " frame id could not be found in the provided URDF");
   }
+
+  this->q_lower = this->model.lowerPositionLimit;
+  this->q_upper = this->model.upperPositionLimit;
+  this->enforce_limits = enforce_limits;
+
+  this->nullspace_gain = nullspace_gain;
+  this->nullspace_q = VectorXd::Zero(this->model.nq);
+  const Eigen::Index n =
+      std::min<Eigen::Index>(nullspace_q.size(), this->nullspace_q.size());
+  this->nullspace_q.head(n) = nullspace_q.head(n);
 }
 
 std::optional<VectorXd> Pin::inverse(const Pose& pose, const VectorXd& q0,
@@ -58,8 +69,26 @@ std::optional<VectorXd> Pin::inverse(const Pose& pose, const VectorXd& q0,
     pinocchio::Data::Matrix6 JJt;
     JJt.noalias() = J * J.transpose();
     JJt.diagonal().array() += this->damp;
-    v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
+
+    if (this->nullspace_gain > 0.0) {
+      Eigen::MatrixXd Jpinv =
+          J.transpose() *
+          JJt.ldlt().solve(pinocchio::Data::Matrix6::Identity());
+      v.noalias() = -Jpinv * err;
+
+      VectorXd dq_ns(model.nv);
+      pinocchio::difference(model, q, this->nullspace_q, dq_ns);
+      const Eigen::MatrixXd N =
+          Eigen::MatrixXd::Identity(model.nv, model.nv) - Jpinv * J;
+      v.noalias() += N * (this->nullspace_gain * dq_ns);
+    } else {
+      v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
+    }
+
     q = pinocchio::integrate(model, q, v * this->DT);
+    if (this->enforce_limits) {
+      q = q.cwiseMax(this->q_lower).cwiseMin(this->q_upper);
+    }
   }
   if (success) {
     return q;