From b9cc55dc49b3bf78732b7e4e684249b5a9121d4e Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 25 Aug 2026 22:39:42 -0700 Subject: [PATCH 01/15] 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 ce6b73cbe..74feed15b 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 95ec05fd5..8a8d51d38 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 59833a977..01d2d8528 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 d6cae733c..4ec992295 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 02/15] 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 b530303a8..74fac5bc5 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 4ec992295..56c2640a4 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 03/15] 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 74fac5bc5..525852c4e 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" From 3017d0ef8ce3f919a2da3497b1289c4bd1d11c95 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 18:17:35 +0000 Subject: [PATCH 04/15] fix(ik): cap the null space gain at 1/dt A posture step larger than the remaining distance to the posture overshoots, so gains above 1/dt made the solver oscillate and fail (FR3 failed from a gain of 25 with dt 0.1). --- python/tests/test_kinematics.py | 13 +++++++++++++ src/rcs/Kinematics.cpp | 7 +++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index 525852c4e..cd38480d5 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -104,3 +104,16 @@ def test_kinematics_nullspace_bias(robot_name): 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}" + + +@pytest.mark.parametrize("gain", [1.0, 10.0, 100.0]) +def test_kinematics_nullspace_large_gain_converges(gain): + robot = rcs.ROBOTS[common.RobotType.FR3] + pin = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, robot.q_home, gain) + pose_home = pin.forward(robot.q_home, NO_TCP_OFFSET) + q_seed = robot.q_home + np.array([0.5, 0.0, 0.5, 0.3, 0.0, 0.0, 0.0]) + + q_sol = pin.inverse(pose_home, q_seed, NO_TCP_OFFSET) + + assert q_sol is not None, f"IK failed with null-space gain {gain}" + assert pin.forward(q_sol, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index dd72e9af0..b55b5f4fa 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -1,5 +1,6 @@ #include "rcs/Kinematics.h" +#include #include #include #include @@ -59,6 +60,8 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, bool success = false; Vector6d err; Eigen::VectorXd v(model.nv); + // a posture step larger than the remaining distance overshoots + const double nullspace_gain = std::min(this->nullspace_gain, 1.0 / this->DT); for (int i = 0;; i++) { pinocchio::forwardKinematics(model, data, q); pinocchio::updateFramePlacements(model, data); @@ -80,7 +83,7 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, JJt.noalias() = J * J.transpose(); JJt.diagonal().array() += this->damp; - if (this->nullspace_gain > 0.0) { + if (nullspace_gain > 0.0) { Eigen::MatrixXd Jpinv = J.transpose() * JJt.ldlt().solve(pinocchio::Data::Matrix6::Identity()); @@ -90,7 +93,7 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, 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); + v.noalias() += N * (nullspace_gain * dq_ns); } else { v.noalias() = -J.transpose() * JJt.ldlt().solve(err); } From f47bce04f82b769fa31ba23342a91d433e7515d9 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 18:18:48 +0000 Subject: [PATCH 05/15] fix(ik): apply the null space posture when the seed reaches the target The solver returned before its first step when the seed already reached the target, so the posture task never acted on a static target. The first iteration now always runs while the posture task is active. --- python/tests/test_kinematics.py | 20 ++++++++++++++++++++ src/rcs/Kinematics.cpp | 3 ++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index cd38480d5..1b2198f86 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -117,3 +117,23 @@ def test_kinematics_nullspace_large_gain_converges(gain): assert q_sol is not None, f"IK failed with null-space gain {gain}" assert pin.forward(q_sol, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) + + +@pytest.mark.parametrize("robot_name", REDUNDANT_ROBOTS) +def test_kinematics_nullspace_applies_when_seed_reaches_pose(robot_name): + """A seed that already reaches the target still moves toward the posture.""" + robot = rcs.ROBOTS[robot_name] + pin_plain = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, np.array([]), 0.0, False) + pin_ns = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, robot.q_home, 10.0, False) + pose_home = pin_plain.forward(robot.q_home, NO_TCP_OFFSET) + q_seed = robot.q_home.copy() + q_seed[0] += 0.5 + q_seed[2] += 0.5 + q_reached = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) + assert q_reached is not None + + q_ns = pin_ns.inverse(pose_home, q_reached, NO_TCP_OFFSET) + + assert q_ns is not None + assert pin_ns.forward(q_ns, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) + assert np.linalg.norm(q_ns - robot.q_home) < np.linalg.norm(q_reached - robot.q_home) diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index b55b5f4fa..e1f7fe29f 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -67,7 +67,8 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, 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) { + const bool posture_pending = nullspace_gain > 0.0 && i == 0; + if (err.norm() < this->eps && !posture_pending) { success = true; break; } From b605f432f34c02ac3965d94a602d0fb9a5e8d987 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 18:20:51 +0000 Subject: [PATCH 06/15] fix(ik): retry without the null space task when it blocks convergence The posture term can keep the solver from reaching the target: with q_home as posture, 31 of 400 reachable FR3 targets failed at gain 1 and 224 at gain 10. The solve is now repeated without the posture before giving up, so enabling the null space never loses a solution. --- include/rcs/Kinematics.h | 2 ++ python/tests/test_kinematics.py | 16 +++++++++++++++ src/rcs/Kinematics.cpp | 36 +++++++++++++++++++-------------- 3 files changed, 39 insertions(+), 15 deletions(-) diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h index 74feed15b..19fab78b5 100644 --- a/include/rcs/Kinematics.h +++ b/include/rcs/Kinematics.h @@ -45,6 +45,8 @@ class Pin : public Kinematics { VectorXd nullspace_q; double nullspace_gain; + bool solve(const pinocchio::SE3& oMdes, VectorXd& q, double nullspace_gain); + public: Pin(const std::string& path, const std::string& frame_id, bool urdf, const VectorXd& nullspace_q = VectorXd(), double nullspace_gain = 0.0, diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index 1b2198f86..490949cfc 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -137,3 +137,19 @@ def test_kinematics_nullspace_applies_when_seed_reaches_pose(robot_name): assert q_ns is not None assert pin_ns.forward(q_ns, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) assert np.linalg.norm(q_ns - robot.q_home) < np.linalg.norm(q_reached - robot.q_home) + + +@pytest.mark.parametrize("robot_name", REDUNDANT_ROBOTS) +def test_kinematics_nullspace_never_loses_solutions(robot_name): + """Targets the plain solver reaches stay reachable with the posture task.""" + robot = rcs.ROBOTS[robot_name] + pin_plain = common.Pin(robot.mjcf_model_path, robot.attachment_site, False) + pin_ns = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, robot.q_home, 10.0) + lower, upper = robot.joint_limits + rng = np.random.default_rng(1) + for _ in range(50): + q_target = rng.uniform(lower, upper) + q_seed = np.clip(q_target + rng.uniform(-0.3, 0.3, q_target.shape), lower, upper) + pose = pin_plain.forward(q_target, NO_TCP_OFFSET) + if pin_plain.inverse(pose, q_seed, NO_TCP_OFFSET) is not None: + assert pin_ns.inverse(pose, q_seed, NO_TCP_OFFSET) is not None diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index e1f7fe29f..c08daf118 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -51,17 +51,30 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { rcs::common::Pose new_pose = pose * tcp_offset.inverse(); - VectorXd q(model.nq); - q.setZero(); - q.head(q0.size()) = q0; const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); + VectorXd q_seed(model.nq); + q_seed.setZero(); + q_seed.head(q0.size()) = q0; + // a posture step larger than the remaining distance overshoots + const double nullspace_gain = std::min(this->nullspace_gain, 1.0 / this->DT); + if (nullspace_gain > 0.0) { + VectorXd q = q_seed; + if (this->solve(oMdes, q, nullspace_gain)) { + return q; + } + } + if (this->solve(oMdes, q_seed, 0.0)) { + return q_seed; + } + return std::nullopt; +} + +bool Pin::solve(const pinocchio::SE3& oMdes, VectorXd& q, + double nullspace_gain) { pinocchio::Data::Matrix6x J(6, model.nv); J.setZero(); - bool success = false; Vector6d err; Eigen::VectorXd v(model.nv); - // a posture step larger than the remaining distance overshoots - const double nullspace_gain = std::min(this->nullspace_gain, 1.0 / this->DT); for (int i = 0;; i++) { pinocchio::forwardKinematics(model, data, q); pinocchio::updateFramePlacements(model, data); @@ -69,12 +82,10 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, err = pinocchio::log6(iMd).toVector(); const bool posture_pending = nullspace_gain > 0.0 && i == 0; if (err.norm() < this->eps && !posture_pending) { - success = true; - break; + return true; } if (i >= this->IT_MAX) { - success = false; - break; + return false; } pinocchio::computeFrameJacobian(model, data, q, this->FRAME_ID, J); pinocchio::Data::Matrix6 Jlog; @@ -104,11 +115,6 @@ std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, q = q.cwiseMax(this->q_lower).cwiseMin(this->q_upper); } } - if (success) { - return q; - } else { - return std::nullopt; - } } Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) { From 0eb9951c09493127e0520fcb8a79c39c7a6f8ade Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 18:22:01 +0000 Subject: [PATCH 07/15] fix(ik): reject frames that do not exist in the model getFrameId returns the number of frames for an unknown name, never -1, so the check never fired and the solver read past the frame array. This is what crashed Pin for the Panda, whose attachment site Pinocchio drops because it sits on a body without a joint. --- python/tests/test_kinematics.py | 8 +++++++- src/rcs/Kinematics.cpp | 9 +++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index 490949cfc..5c53a4e9a 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -5,7 +5,7 @@ from rcs import common # Restrict this test to robot models that are expected to work through the generic Pin binding. -# Panda currently segfaults in the native binding here, and SO101 uses a dedicated IK implementation. +# Pinocchio drops the Panda attachment site as it sits on a body without a joint. PIN_SUPPORTED_ROBOTS = [ common.RobotType.FR3, common.RobotType("XArm7"), @@ -153,3 +153,9 @@ def test_kinematics_nullspace_never_loses_solutions(robot_name): pose = pin_plain.forward(q_target, NO_TCP_OFFSET) if pin_plain.inverse(pose, q_seed, NO_TCP_OFFSET) is not None: assert pin_ns.inverse(pose, q_seed, NO_TCP_OFFSET) is not None + + +def test_kinematics_missing_frame_raises(): + robot = rcs.ROBOTS[common.RobotType.FR3] + with pytest.raises(RuntimeError, match="not_a_frame"): + common.Pin(robot.mjcf_model_path, "not_a_frame", False) diff --git a/src/rcs/Kinematics.cpp b/src/rcs/Kinematics.cpp index c08daf118..4346f2255 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -23,11 +23,12 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, pinocchio::mjcf::buildModel(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"); + if (!model.existFrame(frame_id)) { + throw std::runtime_error(frame_id + + " frame id could not be found in the provided " + + (urdf ? "URDF" : "MJCF")); } + this->FRAME_ID = model.getFrameId(frame_id); this->q_lower = this->model.lowerPositionLimit; this->q_upper = this->model.upperPositionLimit; From 38bd700100354e5714146655c594757b1c6d8802 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 18:26:29 +0000 Subject: [PATCH 08/15] feat(ik): optional base frame for the kinematics poses Pin poses are expressed in the root body of the model, because Pinocchio ignores the pose of the MJCF root body. base_frame expresses them relative to another frame fixed to the world instead, e.g. the robot base when the model mounts the robot on another body. None keeps the current behaviour. --- include/rcs/Kinematics.h | 4 ++- python/rcs/_core/common.pyi | 1 + python/tests/test_kinematics.py | 60 +++++++++++++++++++++++++++++++++ src/pybind/rcs.cpp | 6 ++-- src/rcs/Kinematics.cpp | 19 +++++++++-- 5 files changed, 84 insertions(+), 6 deletions(-) diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h index 19fab78b5..0ffb39001 100644 --- a/include/rcs/Kinematics.h +++ b/include/rcs/Kinematics.h @@ -34,6 +34,7 @@ class Pin : public Kinematics { const double DT = 1e-1; const double damp = 1e-6; int FRAME_ID; + Pose base_pose; pinocchio::Model model; pinocchio::Data data; @@ -50,7 +51,8 @@ class Pin : public Kinematics { public: 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); + bool enforce_limits = true, + const std::optional& base_frame = std::nullopt); 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 f34782b79..adb0e6ac2 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -323,6 +323,7 @@ class Pin(Kinematics): nullspace_q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] = ..., nullspace_gain: float = 0.0, enforce_limits: bool = True, + base_frame: str | None = None, ) -> None: ... def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ... diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index 5c53a4e9a..0702ae645 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -1,3 +1,4 @@ +import mujoco import numpy as np import pytest @@ -159,3 +160,62 @@ def test_kinematics_missing_frame_raises(): robot = rcs.ROBOTS[common.RobotType.FR3] with pytest.raises(RuntimeError, match="not_a_frame"): common.Pin(robot.mjcf_model_path, "not_a_frame", False) + + +MOUNTED_ARM_MJCF = """ + + + + + + + + + + + + + + + + + + + + + + +""" + + +def test_kinematics_base_frame(tmp_path): + """Poses are expressed in base_frame, matching MuJoCo's site pose relative to that body.""" + path = tmp_path / "mounted_arm.xml" + path.write_text(MOUNTED_ARM_MJCF) + pin = common.Pin(str(path), "tip", False, base_frame="robot_base") + q = np.array([0.4, -0.3, 0.5]) + + model = mujoco.MjModel.from_xml_path(str(path)) + data = mujoco.MjData(model) + data.qpos[:] = q + mujoco.mj_forward(model, data) + base = data.body("robot_base") + base_pose = common.Pose(base.xmat.reshape(3, 3), base.xpos) + site_pose = common.Pose(data.site("tip").xmat.reshape(3, 3), data.site("tip").xpos) + expected = base_pose.inverse() * site_pose + + pose = pin.forward(q, NO_TCP_OFFSET) + assert pose.is_close(expected, eps_r=1e-9, eps_t=1e-9) + + tcp_offset = common.Pose(translation=np.array([0.0, 0.0, 0.1])) + pose_tcp = pin.forward(q, tcp_offset) + q_sol: np.ndarray | None = pin.inverse(pose_tcp, q + 0.1, tcp_offset) + assert q_sol is not None + assert pin.forward(q_sol, tcp_offset).is_close(pose_tcp, eps_r=1e-3, eps_t=1e-3) + + +def test_kinematics_base_frame_moved_by_joint_raises(tmp_path): + path = tmp_path / "mounted_arm.xml" + path.write_text(MOUNTED_ARM_MJCF) + with pytest.raises(RuntimeError, match="link2"): + common.Pin(str(path), "tip", False, base_frame="link2") diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 707fc93f3..969a976ab 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -380,11 +380,13 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") .def(py::init(), + const rcs::common::VectorXd&, double, bool, + const std::optional&>(), py::arg("path"), py::arg("frame_id") = "fr3_link8", py::arg("urdf") = false, py::arg("nullspace_q") = rcs::common::VectorXd(), - py::arg("nullspace_gain") = 0.0, py::arg("enforce_limits") = true); + py::arg("nullspace_gain") = 0.0, py::arg("enforce_limits") = true, + py::arg("base_frame") = std::nullopt); 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 4346f2255..dc1ce7053 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -15,7 +15,7 @@ namespace common { Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, const VectorXd& nullspace_q, double nullspace_gain, - bool enforce_limits) + bool enforce_limits, const std::optional& base_frame) : model() { if (urdf) { pinocchio::urdf::buildModel(path, this->model); @@ -29,6 +29,19 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, (urdf ? "URDF" : "MJCF")); } this->FRAME_ID = model.getFrameId(frame_id); + if (base_frame) { + if (!model.existFrame(*base_frame)) { + throw std::runtime_error(*base_frame + + " base frame could not be found in the model"); + } + const pinocchio::Frame& base = model.frames[model.getFrameId(*base_frame)]; + if (base.parentJoint != 0) { + throw std::runtime_error(*base_frame + + " base frame must not be moved by a joint"); + } + this->base_pose = + Pose(base.placement.rotation(), base.placement.translation()); + } this->q_lower = this->model.lowerPositionLimit; this->q_upper = this->model.upperPositionLimit; @@ -51,7 +64,7 @@ Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, std::optional Pin::inverse(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { - rcs::common::Pose new_pose = pose * tcp_offset.inverse(); + rcs::common::Pose new_pose = this->base_pose * pose * tcp_offset.inverse(); const pinocchio::SE3 oMdes(new_pose.rotation_m(), new_pose.translation()); VectorXd q_seed(model.nq); q_seed.setZero(); @@ -128,7 +141,7 @@ Pose Pin::forward(const VectorXd& q0, const Pose& tcp_offset) { data.oMf[this->FRAME_ID].translation()); // apply the tcp offset - return pose * tcp_offset; + return this->base_pose.inverse() * pose * tcp_offset; } } // namespace common From d3abf2f17076f2ea96229ffbd97489b74275acc4 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 18:26:50 +0000 Subject: [PATCH 09/15] 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 0702ae645..afc243b76 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -130,10 +130,10 @@ def test_kinematics_nullspace_applies_when_seed_reaches_pose(robot_name): q_seed = robot.q_home.copy() q_seed[0] += 0.5 q_seed[2] += 0.5 - q_reached = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) + q_reached: np.ndarray | None = pin_plain.inverse(pose_home, q_seed, NO_TCP_OFFSET) assert q_reached is not None - q_ns = pin_ns.inverse(pose_home, q_reached, NO_TCP_OFFSET) + q_ns: np.ndarray | None = pin_ns.inverse(pose_home, q_reached, NO_TCP_OFFSET) assert q_ns is not None assert pin_ns.forward(q_ns, NO_TCP_OFFSET).is_close(pose_home, eps_r=1e-3, eps_t=1e-3) From 03af0afe3bdf4910a15a470fffb0e9d42e62bc05 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 19:01:25 +0000 Subject: [PATCH 10/15] feat(ik): optional joint limits for the clamping Pin clamped to the joint ranges of the MJCF or URDF, which for the FR3 are the physical limits, wider than the recommended limits in the RCS config. joint_limits replaces them, e.g. with the robot config limits, and None keeps the model limits. The constructor is now documented. --- include/rcs/Kinematics.h | 4 +++- python/rcs/_core/common.pyi | 23 ++++++++++++++++++- python/tests/test_kinematics.py | 28 +++++++++++++++++++++++ src/pybind/rcs.cpp | 39 ++++++++++++++++++++++++++------- src/rcs/Kinematics.cpp | 20 ++++++++++++++--- 5 files changed, 101 insertions(+), 13 deletions(-) diff --git a/include/rcs/Kinematics.h b/include/rcs/Kinematics.h index 0ffb39001..41311565c 100644 --- a/include/rcs/Kinematics.h +++ b/include/rcs/Kinematics.h @@ -52,7 +52,9 @@ class Pin : public Kinematics { 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, - const std::optional& base_frame = std::nullopt); + const std::optional& base_frame = std::nullopt, + const std::optional>& + joint_limits = std::nullopt); 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 adb0e6ac2..a9c2380ef 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -324,7 +324,28 @@ class Pin(Kinematics): nullspace_gain: float = 0.0, enforce_limits: bool = True, base_frame: str | None = None, - ) -> None: ... + joint_limits: numpy.ndarray[tuple[typing.Literal[2], M], numpy.dtype[numpy.float64]] | None = None, + ) -> None: + """ + Numerical inverse kinematics (damped least squares) on a Pinocchio model. + + Args: + path: MJCF or URDF file of the robot. + frame_id: Frame whose pose forward and inverse refer to, usually the attachment site. + urdf: Load ``path`` as URDF instead of MJCF. + nullspace_q: Preferred posture of the joints, used as a secondary task in the null space of + the pose task. Missing entries are filled with zeros. + nullspace_gain: Gain of the posture task, 0 disables it. Gains above 1/dt are capped to 1/dt. + If the posture task keeps the solver from reaching the pose, the solve is repeated without it. + enforce_limits: Clamp every solver step to the joint limits. + base_frame: Frame fixed to the world in which poses are expressed. None uses the root body of + the model, whose own pose Pinocchio ignores. + joint_limits: Joint limits for the clamping as a 2 x n matrix, lower limits in the first row and + upper limits in the second, usually the ``joint_limits`` of the robot config. They replace + the limits of the first n joints and must not cover more joints than the model has. None + uses the joint ranges of the MJCF or URDF. Joints with an infinite limit or an empty range + (lower >= upper) are treated as unlimited. Only used if ``enforce_limits`` is True. + """ 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/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index afc243b76..df81db722 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -219,3 +219,31 @@ def test_kinematics_base_frame_moved_by_joint_raises(tmp_path): path.write_text(MOUNTED_ARM_MJCF) with pytest.raises(RuntimeError, match="link2"): common.Pin(str(path), "tip", False, base_frame="link2") + + +def test_kinematics_joint_limits_from_config(): + """Configured limits replace the model limits, so solutions stay inside the narrower config range.""" + robot = rcs.ROBOTS[common.RobotType.FR3] + pin_model_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False) + pin_config_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, joint_limits=robot.joint_limits) + lower, upper = robot.joint_limits + q_target = robot.q_home.copy() + q_target[0] = upper[0] + 0.3 + pose = pin_model_limits.forward(q_target, NO_TCP_OFFSET) + q_seed = robot.q_home.copy() + q_seed[0] = upper[0] + + q_model: np.ndarray | None = pin_model_limits.inverse(pose, q_target, NO_TCP_OFFSET) + q_config: np.ndarray | None = pin_config_limits.inverse(pose, q_seed, NO_TCP_OFFSET) + + assert q_model is not None and q_model[0] > upper[0] + assert q_config is not None + assert np.all((q_config >= lower) & (q_config <= upper)) + assert pin_config_limits.forward(q_config, NO_TCP_OFFSET).is_close(pose, eps_r=1e-3, eps_t=1e-3) + + +def test_kinematics_joint_limits_more_joints_than_model_raises(): + robot = rcs.ROBOTS[common.RobotType("UR5e")] + too_many = np.vstack([-np.ones(robot.dof + 1), np.ones(robot.dof + 1)]) + with pytest.raises(ValueError, match="joint_limits"): + common.Pin(robot.mjcf_model_path, robot.attachment_site, False, joint_limits=too_many) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 969a976ab..14616a41d 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -379,14 +379,37 @@ PYBIND11_MODULE(_core, m) { py::class_>(common, "Pin") - .def(py::init&>(), - py::arg("path"), py::arg("frame_id") = "fr3_link8", - py::arg("urdf") = false, - py::arg("nullspace_q") = rcs::common::VectorXd(), - py::arg("nullspace_gain") = 0.0, py::arg("enforce_limits") = true, - py::arg("base_frame") = std::nullopt); + .def( + py::init< + const std::string&, const std::string&, bool, + const rcs::common::VectorXd&, double, bool, + const std::optional&, + const std::optional>&>(), + R"doc(Numerical inverse kinematics (damped least squares) on a Pinocchio model. + +Args: + path: MJCF or URDF file of the robot. + frame_id: Frame whose pose forward and inverse refer to, usually the attachment site. + urdf: Load ``path`` as URDF instead of MJCF. + nullspace_q: Preferred posture of the joints, used as a secondary task in the null space of + the pose task. Missing entries are filled with zeros. + nullspace_gain: Gain of the posture task, 0 disables it. Gains above 1/dt are capped to 1/dt. + If the posture task keeps the solver from reaching the pose, the solve is repeated without it. + enforce_limits: Clamp every solver step to the joint limits. + base_frame: Frame fixed to the world in which poses are expressed. None uses the root body of + the model, whose own pose Pinocchio ignores. + joint_limits: Joint limits for the clamping as a 2 x n matrix, lower limits in the first row and + upper limits in the second, usually the ``joint_limits`` of the robot config. They replace + the limits of the first n joints and must not cover more joints than the model has. None + uses the joint ranges of the MJCF or URDF. Joints with an infinite limit or an empty range + (lower >= upper) are treated as unlimited. Only used if ``enforce_limits`` is True. +)doc", + py::arg("path"), py::arg("frame_id") = "fr3_link8", + py::arg("urdf") = false, + py::arg("nullspace_q") = rcs::common::VectorXd(), + py::arg("nullspace_gain") = 0.0, py::arg("enforce_limits") = true, + py::arg("base_frame") = std::nullopt, + py::arg("joint_limits") = std::nullopt); 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 dc1ce7053..7b9cb329c 100644 --- a/src/rcs/Kinematics.cpp +++ b/src/rcs/Kinematics.cpp @@ -9,13 +9,17 @@ #include #include #include +#include +#include namespace rcs { namespace common { -Pin::Pin(const std::string& path, const std::string& frame_id, bool urdf, - const VectorXd& nullspace_q, double nullspace_gain, - bool enforce_limits, const std::optional& base_frame) +Pin::Pin( + const std::string& path, const std::string& frame_id, bool urdf, + const VectorXd& nullspace_q, double nullspace_gain, bool enforce_limits, + const std::optional& base_frame, + const std::optional>& joint_limits) : model() { if (urdf) { pinocchio::urdf::buildModel(path, this->model); @@ -45,6 +49,16 @@ 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; + if (joint_limits) { + const Eigen::Index n = joint_limits->cols(); + if (n > this->model.nq) { + throw std::invalid_argument("joint_limits has " + std::to_string(n) + + " columns but the model only has " + + std::to_string(this->model.nq) + " joints"); + } + this->q_lower.head(n) = joint_limits->row(0).transpose(); + this->q_upper.head(n) = joint_limits->row(1).transpose(); + } 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]) || From 8d13ea5bbf008eeac4e88bfb2af0a27d2f4e633d Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 19:03:22 +0000 Subject: [PATCH 11/15] feat(ik): clamp to the configured joint limits All Pin constructions now pass the joint_limits of the robot config, so IK solutions stay inside the same limits as the gym action and observation spaces. For the FR3 these are the recommended limits, which are narrower than the physical ranges in the MJCF. --- README.md | 1 + docs/getting_started/index.md | 2 +- examples/fr3/fr3_direct_control.py | 2 ++ examples/fr3/fr3_env_cartesian_control.py | 1 + examples/fr3/fr3_env_joint_control.py | 1 + examples/fr3/fr3_readme.py | 1 + examples/so101/so101_env_cartesian_control.py | 1 + examples/so101/so101_env_joint_control.py | 1 + examples/ur5e/ur5e_env_cartesian_control.py | 1 + examples/ur5e/ur5e_env_joint_control.py | 1 + examples/xarm7/xarm7_env_cartesian_control.py | 1 + examples/xarm7/xarm7_env_joint_control.py | 1 + examples/yam/yam_env_cartesian_control.py | 1 + extensions/rcs_fr3/src/rcs_fr3/creators.py | 1 + extensions/rcs_panda/src/rcs_panda/creators.py | 1 + extensions/rcs_ur5e/src/rcs_ur5e/creators.py | 1 + extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py | 1 + extensions/rcs_xarm7/src/rcs_xarm7/creators.py | 1 + extensions/rcs_yam/README.md | 2 +- extensions/rcs_yam/src/rcs_yam/creators.py | 1 + extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py | 1 + python/rcs/envs/scenes.py | 1 + python/rcs/lerobot_joint_converter.py | 1 + python/tests/test_rpc.py | 1 + python/tests/test_sim_envs.py | 1 + 25 files changed, 26 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 864c7673d..1ac6724ab 100644 --- a/README.md +++ b/README.md @@ -82,6 +82,7 @@ if __name__ == "__main__": ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) # base env diff --git a/docs/getting_started/index.md b/docs/getting_started/index.md index d7e60720a..39e431cda 100644 --- a/docs/getting_started/index.md +++ b/docs/getting_started/index.md @@ -71,7 +71,7 @@ import numpy as np # Load simulation scene robot_meta = rcs.ROBOTS[rcs.common.RobotType.FR3] simulation = sim.Sim(rcs.SCENE_PATHS["empty_world"]) -ik = rcs.common.Pin(robot_meta.mjcf_model_path, robot_meta.attachment_site) +ik = rcs.common.Pin(robot_meta.mjcf_model_path, robot_meta.attachment_site, joint_limits=robot_meta.joint_limits) # Configure robot cfg = sim.SimRobotConfig() diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index 7edb1743f..342f1dec9 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/examples/fr3/fr3_direct_control.py @@ -79,6 +79,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) @@ -115,6 +116,7 @@ def main(): fr3_cfg.kinematic_model_path, fr3_cfg.attachment_site, urdf=fr3_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=fr3_cfg.joint_limits, ) robot = hw.Franka(fr3_cfg, ik) diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py index ab0b5fea2..982292fc5 100644 --- a/examples/fr3/fr3_env_cartesian_control.py +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -59,6 +59,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py index 094358d23..50ba5e990 100644 --- a/examples/fr3/fr3_env_joint_control.py +++ b/examples/fr3/fr3_env_joint_control.py @@ -59,6 +59,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(cfg) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py index 95d08d2cd..195955b32 100644 --- a/examples/fr3/fr3_readme.py +++ b/examples/fr3/fr3_readme.py @@ -41,6 +41,7 @@ ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) # base env diff --git a/examples/so101/so101_env_cartesian_control.py b/examples/so101/so101_env_cartesian_control.py index fe8facd0d..62f29618d 100644 --- a/examples/so101/so101_env_cartesian_control.py +++ b/examples/so101/so101_env_cartesian_control.py @@ -40,6 +40,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/so101/so101_env_joint_control.py b/examples/so101/so101_env_joint_control.py index 470dea926..d117a37cc 100644 --- a/examples/so101/so101_env_joint_control.py +++ b/examples/so101/so101_env_joint_control.py @@ -38,6 +38,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(cfg) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/ur5e/ur5e_env_cartesian_control.py b/examples/ur5e/ur5e_env_cartesian_control.py index c323df624..6b1a261ac 100644 --- a/examples/ur5e/ur5e_env_cartesian_control.py +++ b/examples/ur5e/ur5e_env_cartesian_control.py @@ -59,6 +59,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/ur5e/ur5e_env_joint_control.py b/examples/ur5e/ur5e_env_joint_control.py index 8e32dd497..635920381 100644 --- a/examples/ur5e/ur5e_env_joint_control.py +++ b/examples/ur5e/ur5e_env_joint_control.py @@ -58,6 +58,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(sim_cfg_data) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py index a9dc0ef73..95104b71b 100644 --- a/examples/xarm7/xarm7_env_cartesian_control.py +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -64,6 +64,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/examples/xarm7/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py index 7f77575aa..c27edbebc 100644 --- a/examples/xarm7/xarm7_env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -61,6 +61,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) mjmodel = scene.create_model(sim_cfg_data) simulation = sim.Sim(mjmodel, sim_cfg) diff --git a/examples/yam/yam_env_cartesian_control.py b/examples/yam/yam_env_cartesian_control.py index ec99726ee..c2d1d3845 100644 --- a/examples/yam/yam_env_cartesian_control.py +++ b/examples/yam/yam_env_cartesian_control.py @@ -80,6 +80,7 @@ def main(): ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 36e4f3f9c..57a1f7b05 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -188,6 +188,7 @@ def create_env(self, cfg: FR3HardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = hw.Franka(cfg.robot_cfg, ik) diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py index 28bace5a0..9ac11494a 100644 --- a/extensions/rcs_panda/src/rcs_panda/creators.py +++ b/extensions/rcs_panda/src/rcs_panda/creators.py @@ -148,6 +148,7 @@ def create_env(self, cfg: PandaHardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = hw.Franka(cfg.robot_cfg, ik) diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py index f89f3880b..aa943714c 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py @@ -109,6 +109,7 @@ def create_env(self, cfg: UR5eHardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = UR5e(cfg.robot_cfg, ik) env: gym.Env = HardwareEnv(frequency=cfg.frequency) diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py index 6c58b37b1..55cfa59d1 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/test_robot.py @@ -12,6 +12,7 @@ robot_config.kinematic_model_path, robot_config.attachment_site, urdf=robot_config.kinematic_model_path.endswith(".urdf"), + joint_limits=robot_config.joint_limits, ) robot = UR5e(robot_config, ik) print(f"Robot joint positions: {robot.get_joint_position()}") diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index a4edb2beb..36c198154 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -104,6 +104,7 @@ def create_env(self, cfg: XArm7HardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = XArm7(cfg=cfg.robot_cfg, ik=ik) env: gym.Env = HardwareEnv(frequency=cfg.frequency) diff --git a/extensions/rcs_yam/README.md b/extensions/rcs_yam/README.md index 6be612dc2..12539ec7b 100644 --- a/extensions/rcs_yam/README.md +++ b/extensions/rcs_yam/README.md @@ -91,7 +91,7 @@ from rcs import common from rcs_yam.hw import Yam, YamConfig, YamGripper cfg = YamConfig(channel="can0", async_control=False, dof=6, ...) -ik = common.Pin(cfg.kinematic_model_path, cfg.attachment_site) +ik = common.Pin(cfg.kinematic_model_path, cfg.attachment_site, joint_limits=cfg.joint_limits) robot = Yam(cfg, ik) gripper = YamGripper(common.GripperConfig(gripper_type=common.GripperType("Yam")), robot) diff --git a/extensions/rcs_yam/src/rcs_yam/creators.py b/extensions/rcs_yam/src/rcs_yam/creators.py index aa42554a2..3d90ede3b 100644 --- a/extensions/rcs_yam/src/rcs_yam/creators.py +++ b/extensions/rcs_yam/src/rcs_yam/creators.py @@ -103,6 +103,7 @@ def create_env(self, cfg: YamHardwareEnvCreatorConfig) -> gym.Env: cfg.robot_cfg.kinematic_model_path, cfg.robot_cfg.attachment_site, urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"), + joint_limits=cfg.robot_cfg.joint_limits, ) robot = Yam(cfg.robot_cfg, ik) env: gym.Env = HardwareEnv(frequency=cfg.frequency) diff --git a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py index 75a9908fb..d745ecb7e 100644 --- a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py +++ b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py @@ -30,6 +30,7 @@ robot_config.kinematic_model_path, robot_config.attachment_site, urdf=robot_config.kinematic_model_path.endswith(".urdf"), + joint_limits=robot_config.joint_limits, ) robot = Yam(robot_config, ik) gripper = YamGripper(common.GripperConfig(gripper_type=gripper_type), robot) diff --git a/python/rcs/envs/scenes.py b/python/rcs/envs/scenes.py index 16ca88f9d..8b7d6e4de 100644 --- a/python/rcs/envs/scenes.py +++ b/python/rcs/envs/scenes.py @@ -353,6 +353,7 @@ def create_env_from_model(self, cfg: SimEnvCreatorConfig, mjmodel: MjModel) -> g ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=prefixed_cfg.robot_cfgs[robot_name].joint_limits, ) env = self.add_robot_env(prefixed_cfg, robot_name, env, simulation, ik) diff --git a/python/rcs/lerobot_joint_converter.py b/python/rcs/lerobot_joint_converter.py index 2e57a06d3..f6015539e 100644 --- a/python/rcs/lerobot_joint_converter.py +++ b/python/rcs/lerobot_joint_converter.py @@ -124,6 +124,7 @@ def __init__( self.ik = rcs.common.Pin( rcs.ROBOTS[robot_type].mjcf_model_path, rcs.ROBOTS[robot_type].attachment_site, + joint_limits=rcs.ROBOTS[robot_type].joint_limits, ) self.camera_resizers = { camera.name: v2.Resize(camera.resolution) # type: ignore[name-defined] # noqa: F821 diff --git a/python/tests/test_rpc.py b/python/tests/test_rpc.py index b45713f52..dbbc9c379 100644 --- a/python/tests/test_rpc.py +++ b/python/tests/test_rpc.py @@ -50,6 +50,7 @@ def build_rpc_env() -> gym.Env: ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) env: gym.Env = SimEnv(simulation) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 325d1ef4b..4a8428107 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -55,6 +55,7 @@ def build_single_robot_env( ik = rcs.common.Pin( kinematic_model_path, attachment_site, + joint_limits=robot_cfg.joint_limits, ) env: gym.Env = SimEnv(simulation) From cb8f7f3b7bf85eacb76259470c4683fb9df5c7f3 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Tue, 13 Oct 2026 19:03:27 +0000 Subject: [PATCH 12/15] fix(panda): set the joint limits in the default hardware config The Panda limits in the C++ PandaConfig shadow the RobotConfig field and are not visible from Python, so the default hardware config reported the FR3 limits to the gym spaces and the IK. It now sets the Panda limits like the FR3 config does. --- extensions/rcs_panda/src/rcs_panda/configs.py | 1 + 1 file changed, 1 insertion(+) diff --git a/extensions/rcs_panda/src/rcs_panda/configs.py b/extensions/rcs_panda/src/rcs_panda/configs.py index d90453e59..0c6ff8835 100644 --- a/extensions/rcs_panda/src/rcs_panda/configs.py +++ b/extensions/rcs_panda/src/rcs_panda/configs.py @@ -24,6 +24,7 @@ def config(self) -> PandaHardwareEnvCreatorConfig: robot_cfg.kinematic_model_path = rcs.ROBOTS[RobotType.Panda].mjcf_model_path robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset()) robot_cfg.attachment_site = rcs.ROBOTS[RobotType.Panda].attachment_site + robot_cfg.joint_limits = rcs.ROBOTS[RobotType.Panda].joint_limits robot_cfg.speed_factor = 0.1 robot_cfg.ik_solver = hw.IKSolver.rcs_ik robot_cfg.async_control = False From 8fb1d48b44e737014e4d144fb1f3b947abbdaa3e Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 14 Oct 2026 10:46:26 -0700 Subject: [PATCH 13/15] chore(fr3): joint limts changed to hardware specification --- python/rcs/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index d8ed06a9f..7a4776a39 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -110,8 +110,8 @@ class RobotMetaConfig: q_home=np.array([0.0, -np.pi / 4, 0.0, -3 * np.pi / 4, 0.0, np.pi / 2, 0.0]), joint_limits=np.array( [ - [-2.3476, -1.5454, -2.4937, -2.7714, -2.5100, 0.7773, -2.7045], - [2.3476, 1.5454, 2.4937, -0.4226, 2.5100, 4.2841, 2.7045], + [-2.9007, -1.8361, -2.9007, -3.0770, -2.8763, 0.4398, -3.0508], + [2.9007, 1.8361, 2.9007, -0.1169, 2.8763, 4.6216, 3.0508], ] ), ), From 367faa6030f6799f78e3e0342106f0c080c87be3 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 14 Oct 2026 17:51:46 +0000 Subject: [PATCH 14/15] style: joint limits test independent of the configured fr3 limits With the FR3 config limits now equal to the MJCF limits the test passed only because its seed already reached the target. It now uses its own narrower limits. --- python/tests/test_kinematics.py | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py index df81db722..a0662ff23 100644 --- a/python/tests/test_kinematics.py +++ b/python/tests/test_kinematics.py @@ -221,25 +221,31 @@ def test_kinematics_base_frame_moved_by_joint_raises(tmp_path): common.Pin(str(path), "tip", False, base_frame="link2") -def test_kinematics_joint_limits_from_config(): - """Configured limits replace the model limits, so solutions stay inside the narrower config range.""" +def test_kinematics_joint_limits_override_model_limits(): + """Given joint limits replace the model limits, so solutions stay inside a range narrower than the MJCF.""" robot = rcs.ROBOTS[common.RobotType.FR3] + narrow_limits = np.array( + [ + [-2.3476, -1.5454, -2.4937, -2.7714, -2.5100, 0.7773, -2.7045], + [2.3476, 1.5454, 2.4937, -0.4226, 2.5100, 4.2841, 2.7045], + ] + ) pin_model_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False) - pin_config_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, joint_limits=robot.joint_limits) - lower, upper = robot.joint_limits + pin_narrow_limits = common.Pin(robot.mjcf_model_path, robot.attachment_site, False, joint_limits=narrow_limits) + lower, upper = narrow_limits q_target = robot.q_home.copy() q_target[0] = upper[0] + 0.3 pose = pin_model_limits.forward(q_target, NO_TCP_OFFSET) q_seed = robot.q_home.copy() q_seed[0] = upper[0] - q_model: np.ndarray | None = pin_model_limits.inverse(pose, q_target, NO_TCP_OFFSET) - q_config: np.ndarray | None = pin_config_limits.inverse(pose, q_seed, NO_TCP_OFFSET) + q_model: np.ndarray | None = pin_model_limits.inverse(pose, q_seed, NO_TCP_OFFSET) + q_narrow: np.ndarray | None = pin_narrow_limits.inverse(pose, q_seed, NO_TCP_OFFSET) assert q_model is not None and q_model[0] > upper[0] - assert q_config is not None - assert np.all((q_config >= lower) & (q_config <= upper)) - assert pin_config_limits.forward(q_config, NO_TCP_OFFSET).is_close(pose, eps_r=1e-3, eps_t=1e-3) + assert q_narrow is not None + assert np.all((q_narrow >= lower) & (q_narrow <= upper)) + assert pin_narrow_limits.forward(q_narrow, NO_TCP_OFFSET).is_close(pose, eps_r=1e-3, eps_t=1e-3) def test_kinematics_joint_limits_more_joints_than_model_raises(): From 70adb127af657cf8f8f9df7543917caf3f9eb6f9 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 14 Oct 2026 11:41:16 -0700 Subject: [PATCH 15/15] ci: refactor eigen clone in tarball download --- CMakeLists.txt | 5 ++--- extensions/rcs_fr3/CMakeLists.txt | 5 ++--- extensions/rcs_panda/CMakeLists.txt | 5 ++--- extensions/rcs_so101/CMakeLists.txt | 5 ++--- 4 files changed, 8 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bc18ee7e1..9ccfe3cdb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,9 +45,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL ) FetchContent_Declare( diff --git a/extensions/rcs_fr3/CMakeLists.txt b/extensions/rcs_fr3/CMakeLists.txt index b5365fcb9..7cf1ecdde 100644 --- a/extensions/rcs_fr3/CMakeLists.txt +++ b/extensions/rcs_fr3/CMakeLists.txt @@ -62,9 +62,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL OVERRIDE_FIND_PACKAGE ) diff --git a/extensions/rcs_panda/CMakeLists.txt b/extensions/rcs_panda/CMakeLists.txt index 63731d07f..49b4e6e12 100644 --- a/extensions/rcs_panda/CMakeLists.txt +++ b/extensions/rcs_panda/CMakeLists.txt @@ -68,9 +68,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL OVERRIDE_FIND_PACKAGE ) diff --git a/extensions/rcs_so101/CMakeLists.txt b/extensions/rcs_so101/CMakeLists.txt index 40bae4003..0c9b13b56 100644 --- a/extensions/rcs_so101/CMakeLists.txt +++ b/extensions/rcs_so101/CMakeLists.txt @@ -51,9 +51,8 @@ FetchContent_Declare(pybind11 EXCLUDE_FROM_ALL ) FetchContent_Declare(Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3.4.1 - GIT_PROGRESS TRUE + # release tarball instead of a git clone: gitlab.com often rejects clones of eigen due to load + URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz EXCLUDE_FROM_ALL ) FetchContent_MakeAvailable(pybind11 Eigen3)