fix(models,ik): correct YuMi gripper parents and IK failure-path q compaction

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)
 

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,

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()
 

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]),