From b9cc55dc49b3bf78732b7e4e684249b5a9121d4e Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 25 Aug 2026 22:39:42 -0700 Subject: [PATCH 1/3] feat(pin ik): nullspace and joint limits --- include/rcs/Kinematics.h | 11 ++++++++++- python/rcs/_core/common.pyi | 10 +++++++++- src/pybind/rcs.cpp | 7 +++++-- src/rcs/Kinematics.cpp | 35 ++++++++++++++++++++++++++++++++--- 4 files changed, 56 insertions(+), 7 deletions(-) diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h index ce6b73cb..74feed15 100644 --- a/include/rcs/Kinematics.h +++ b/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 inverse( const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()) override; diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index 95ec05fd..8a8d51d3 100644 --- a/python/rcs/_core/common.pyi +++ b/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]]: ... diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 59833a97..01d2d852 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -378,9 +378,12 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") - .def(py::init(), + .def(py::init(), 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(common, "RobotType") .def_readonly_static("FR3", &rcs::common::RobotType::FR3) diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index d6cae733..4ec99229 100644 --- a/src/rcs/Kinematics.cpp +++ b/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(nullspace_q.size(), this->nullspace_q.size()); + this->nullspace_q.head(n) = nullspace_q.head(n); } std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, @@ -58,8 +69,26 @@ std::optional 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; From ac3e755beafdcdfc5d5973d6527545d8d8272eec Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 26 Aug 2026 10:24:55 -0700 Subject: [PATCH 2/3] fix(ik): non existing limits in mjcf are inf'ed --- python/tests/test_kinematics.py | 88 +++++++++++++++++++++++---------- src/rcs/Kinematics.cpp | 10 ++++ 2 files changed, 72 insertions(+), 26 deletions(-) diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index b530303a..74fac5bc 100644 --- a/python/tests/test_kinematics.py +++ b/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 = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) + q_ns = 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}" diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index 4ec99229..56c2640a 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -1,5 +1,7 @@ #include "rcs/Kinematics.h" +#include +#include #include #include #include @@ -28,6 +30,14 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, this->q_lower = this->model.lowerPositionLimit; this->q_upper = this->model.upperPositionLimit; + const double inf = std::numeric_limits::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; From 2d4734109bc247ead3df54a20cecad3a2608e662 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 26 Aug 2026 10:40:37 -0700 Subject: [PATCH 3/3] style: fix mypy --- python/tests/test_kinematics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index 74fac5bc..525852c4 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -90,8 +90,8 @@ def test_kinematics_nullspace_bias(robot_name): q_seed[2] += 0.5 q_seed[3] += 0.3 - q_plain = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) - q_ns = pin_ns.inverse(pose_home, q_seed, NO_TCP_OFFSET) + 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"