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/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_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() 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]),