From 84bef89d97b603ee08a4c0084224b3904eb29cc6 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 16:39:31 +1000 Subject: [PATCH 1/2] fix(models): correct YuMi gripper parent links r_gripper and l_gripper were wired to the opposite arm's gripper base link, so ikine_LM(end='r_gripper') silently solved for the left arm and vice versa. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/models/URDF/YuMi.py | 4 ++-- tests/test_BaseRobot.py | 12 ++++++++++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/roboticstoolbox/models/URDF/YuMi.py b/src/roboticstoolbox/models/URDF/YuMi.py index b10cf5419..6963af411 100644 --- a/src/roboticstoolbox/models/URDF/YuMi.py +++ b/src/roboticstoolbox/models/URDF/YuMi.py @@ -52,8 +52,8 @@ def __init__(self): l_gripper_links = [link for link in links if link.parent == gripper_l_base] # New intermediate links - r_gripper = Link(name="r_gripper", parent=gripper_l_base) - l_gripper = Link(name="l_gripper", parent=gripper_r_base) + r_gripper = Link(name="r_gripper", parent=gripper_r_base) + l_gripper = Link(name="l_gripper", parent=gripper_l_base) links.append(r_gripper) links.append(l_gripper) diff --git a/tests/test_BaseRobot.py b/tests/test_BaseRobot.py index 09f059271..192b5dc9a 100644 --- a/tests/test_BaseRobot.py +++ b/tests/test_BaseRobot.py @@ -492,6 +492,18 @@ def test_limits(self): self.assertEqual(end.name, "r_gripper") + def test_yumi_gripper_parents(self): + # r_gripper/l_gripper must sit under their own arm's final link, not + # the other arm's -- previously swapped, so end='r_gripper' silently + # solved for the left arm and vice versa. + r = rtb.models.YuMi() + + r_gripper = r._getlink("r_gripper") + l_gripper = r._getlink("l_gripper") + + self.assertEqual(r_gripper.parent.name, "gripper_r_base") + self.assertEqual(l_gripper.parent.name, "gripper_l_base") + def test_limits2(self): r = rtb.models.Panda() From b4eec804c664e4a35a84718744d48d5f7640678d Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 16:40:08 +1000 Subject: [PATCH 2/2] fix(ik): compact q on IK failure return, matching the success path IKSolver._solve() compacted q via ets.jindices on success but returned the raw zero-padded global-length vector on failure. Invisible for a sub-chain whose jindex happens to start at 0, but for one that doesn't (e.g. YuMi's l_gripper, jindex 7-13) the failed solution came back at the wrong length, breaking any caller expecting length == ets.n. Co-Authored-By: Claude Sonnet 5 --- src/roboticstoolbox/robot/IK.py | 2 +- tests/test_IK.py | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 075412b3c..588d78951 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -344,7 +344,7 @@ def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution: reason += ", solution found but violates joint limits" return IKSolution( - q=q, + q=q[ets.jindices], success=False, iterations=total_i, searches=self.slimit, diff --git a/tests/test_IK.py b/tests/test_IK.py index 7c5858fef..8d2120842 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -828,6 +828,25 @@ def test_sol_print5(self): self.assertEqual(s, ans) + def test_ik_lm_failure_returns_compact_q(self): + # regression: the failure-return branch of IK.py's _solve() must + # compact q via ets.jindices, just like the success branch does -- + # otherwise a solver on a sub-chain whose jindex doesn't start at 0 + # (e.g. YuMi's l_gripper, jindex 7-13) returns a zero-padded, + # wrong-length q on failure instead of length ets.n. + from spatialmath import SE3 + + yumi = rtb.models.YuMi() + ets = yumi.ets(end="l_gripper") + + Tep = SE3(0.6, -0.2, 0.3) * SE3.Rx(0.2) + + solver = rtb.IK_LM(ilimit=1, slimit=1) + sol = solver.solve(ets, Tep) + + self.assertFalse(sol.success) + self.assertEqual(sol.q.shape[0], ets.n) + def test_iter_iksol(self): sol = rtb.IKSolution( np.array([1.0, 2.0, 3.0]),