feat(pin ik): nullspace and joint limits

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]]: ...

python/tests/test_kinematics.py

@@ -14,57 +14,93 @@
     common.RobotType("Yam"),
 ]
 
+# Robots with a genuine redundant DOF (7-DoF arms), where null-space biasing has room to act.
+REDUNDANT_ROBOTS = [
+    common.RobotType.FR3,
+    common.RobotType("XArm7"),
+]
+
+# Identity pose / no TCP offset, reused across tests.
+NO_TCP_OFFSET = common.Pose()
+
 
 @pytest.mark.parametrize("robot_name", PIN_SUPPORTED_ROBOTS)
 def test_kinematics_identity(robot_name):
     robot = rcs.ROBOTS[robot_name]
-
-    # Determine model path and type
     model_path = robot.mjcf_model_path
-
     frame_id = robot.attachment_site
+    q_home = robot.q_home
 
-    # Initialize Pinocchio interface
+    # Default Pin: limit-clamping on, no null-space bias.
     try:
         pin = common.Pin(model_path, frame_id, False)
     except Exception as e:
         pytest.fail(f"Failed to initialize Pin for {robot_name}: {e}")
 
-    q_home = robot.q_home
-
-    # Test 1: FK at home
-    # Identity pose (no TCP offset)
-    tcp_offset = common.Pose()
-
-    pose_home = pin.forward(q_home, tcp_offset)
+    # Test 1: FK at home.
+    pose_home = pin.forward(q_home, NO_TCP_OFFSET)
     assert isinstance(pose_home, common.Pose)
 
-    # Test 2: IK at home pose should return a solution (ideally close to q_home, but IK is redundant)
-    # We use q_home as initial guess
-    q_sol: np.ndarray | None = pin.inverse(pose_home, q_home, tcp_offset)
-
+    # Test 2: IK at the home pose returns a solution reaching that pose. The home
+    # configuration is within the joint limits, so clamping does not interfere.
+    q_sol: np.ndarray | None = pin.inverse(pose_home, q_home, NO_TCP_OFFSET)
     assert q_sol is not None, "IK failed for home pose"
 
-    # Verify the solution with FK
-    pose_sol = pin.forward(q_sol, tcp_offset)
-
-    # Check if pose_sol is close to pose_home
+    pose_sol = pin.forward(q_sol, NO_TCP_OFFSET)
     assert pose_sol.is_close(
         pose_home, eps_r=1e-4, eps_t=1e-4
     ), f"FK(IK(pose)) does not match pose.\nOriginal: {pose_home}\nResult: {pose_sol}"
 
-    # Test 3: Perturbed configuration
-    # Add small noise to q_home to test non-trivial pose
-    # Ensure we stay within limits if possible, but for small noise it should be fine
+    # Test 3: Perturbed configuration. We disable limit clamping here so that
+    # reachability of FK(q_perturbed) does not depend on how close q_home sits to
+    # a joint limit (e.g. SO101), keeping this a pure IK-convergence check.
+    pin_free = common.Pin(model_path, frame_id, False, np.array([]), 0.0, False)
+
     np.random.seed(42)
     q_perturbed = q_home + np.random.uniform(-0.1, 0.1, size=q_home.shape)
 
-    pose_perturbed = pin.forward(q_perturbed, tcp_offset)  # type: ignore
-    q_sol_perturbed: np.ndarray | None = pin.inverse(pose_perturbed, q_home, tcp_offset)  # Use q_home as seed
-
+    pose_perturbed = pin_free.forward(q_perturbed, NO_TCP_OFFSET)
+    q_sol_perturbed: np.ndarray | None = pin_free.inverse(pose_perturbed, q_home, NO_TCP_OFFSET)
     assert q_sol_perturbed is not None, "IK failed for perturbed pose"
 
-    pose_sol_perturbed = pin.forward(q_sol_perturbed, tcp_offset)
+    pose_sol_perturbed = pin_free.forward(q_sol_perturbed, NO_TCP_OFFSET)
     assert pose_sol_perturbed.is_close(
         pose_perturbed, eps_r=1e-3, eps_t=1e-3
     ), f"FK(IK(perturbed_pose)) does not match.\nOriginal: {pose_perturbed}\nResult: {pose_sol_perturbed}"
+
+
+@pytest.mark.parametrize("robot_name", REDUNDANT_ROBOTS)
+def test_kinematics_nullspace_bias(robot_name):
+    """A null-space target biases the redundant DOF toward the preferred posture
+    without changing the achieved end-effector pose."""
+    robot = rcs.ROBOTS[robot_name]
+    model_path = robot.mjcf_model_path
+    frame_id = robot.attachment_site
+    q_home = robot.q_home
+
+    # Clamping off on both so the comparison isolates the null-space term.
+    pin_plain = common.Pin(model_path, frame_id, False, np.array([]), 0.0, False)
+    pin_ns = common.Pin(model_path, frame_id, False, q_home, 2.0, False)  # bias toward home
+
+    # Target the home pose; a seed away from home exercises the redundancy so the
+    # two solvers can settle on different configurations for the same pose.
+    pose_home = pin_plain.forward(q_home, NO_TCP_OFFSET)
+    q_seed = q_home.copy()
+    q_seed[0] += 0.5
+    q_seed[2] += 0.5
+    q_seed[3] += 0.3
+
+    q_plain: np.ndarray | None = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET)
+    q_ns: np.ndarray | None = pin_ns.inverse(pose_home, q_seed, NO_TCP_OFFSET)
+
+    assert q_plain is not None, "plain IK failed"
+    assert q_ns is not None, "null-space IK failed"
+
+    # Both solutions must reach the same end-effector pose.
+    assert pin_plain.forward(q_plain, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3)
+    assert pin_ns.forward(q_ns, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3)
+
+    # The null-space-biased solution sits closer to the preferred (home) posture.
+    d_plain = float(np.linalg.norm(q_plain - q_home))
+    d_ns = float(np.linalg.norm(q_ns - q_home))
+    assert d_ns < d_plain, f"null-space bias did not pull toward home: d_ns={d_ns} vs d_plain={d_plain}"

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

@@ -1,5 +1,7 @@
 #include "rcs/Kinematics.h"
 
+#include <cmath>
+#include <limits>
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
 #include <pinocchio/algorithm/joint-configuration.hpp>
@@ -10,8 +12,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 +27,24 @@ 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;
+  const double inf = std::numeric_limits<double>::infinity();
+  for (Eigen::Index i = 0; i < this->q_lower.size(); i++) {
+    if (!std::isfinite(this->q_lower[i]) || !std::isfinite(this->q_upper[i]) ||
+        this->q_lower[i] >= this->q_upper[i]) {
+      this->q_lower[i] = -inf;
+      this->q_upper[i] = inf;
+    }
+  }
+  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 +79,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;