diff --git a/src/roboticstoolbox/ets/fknm.py b/src/roboticstoolbox/ets/fknm.py index a806bf282..e90449c1a 100644 --- a/src/roboticstoolbox/ets/fknm.py +++ b/src/roboticstoolbox/ets/fknm.py @@ -397,10 +397,38 @@ def Angle_Axis(T: Any, Td: Any) -> Any: return _c_Angle_Axis(T, Td) -def Robot_link_T(link_ets: Any, link_scene_nodes: Any, q0: Any, q: Any) -> None: - """Update per-link scene-node transforms via C; no-op when C absent (visualisation only).""" +def Robot_link_T( + link_ets: Any, link_scene_nodes: Any, q0: Any, q: Any, _data: Any = None +) -> None: + """ + Update the scene-node transform of every link of a robot + + :param link_ets: the compiled ETS handle of each link, used by the C path + :param link_scene_nodes: the scene-node transform (4x4 array) of each link, + updated in place + :param q0: the robot's joint coordinates, used if ``q`` is None + :param q: joint coordinates, or None + :param _data: the ``ETS.data`` of each link, used by the Python path + + The scene-node transforms place the collision and visual geometry of the + links, so the Python path must update them as well as the C path: leaving + them stale makes collision checking and distance queries wrong. + """ if _C_AVAILABLE: _c_Robot_link_T(link_ets, link_scene_nodes, q0, q) + return + + if _data is None: + raise ValueError("_data is required when the compiled extension is absent") + + # one row, which also copes with the empty q of a gripper with no joints + q = np.asarray(q0 if q is None else q, dtype=float).reshape(1, -1) + for data, T in zip(_data, link_scene_nodes): + if len(data) == 0: + # a link with no elementary transforms, e.g. a URDF base link + T[:] = np.eye(4) + else: + T[:] = _python_fkine(data, q, None, None, False) def r2q(R: Any) -> Any: diff --git a/src/roboticstoolbox/robot/BaseRobot.py b/src/roboticstoolbox/robot/BaseRobot.py index 6971a362c..9a9f0f7d0 100644 --- a/src/roboticstoolbox/robot/BaseRobot.py +++ b/src/roboticstoolbox/robot/BaseRobot.py @@ -1966,7 +1966,13 @@ def get_link_ets() -> Any: def get_link_scene_node() -> Any: return [link._T_reference for link in self.links] - Robot_link_T(get_link_ets(), get_link_scene_node(), self._q, q) + Robot_link_T( + get_link_ets(), + get_link_scene_node(), + self._q, + q, + _data=[link.ets.data for link in self.links], + ) for gripper in self.grippers: gripper._update_link_tf() diff --git a/src/roboticstoolbox/robot/Gripper.py b/src/roboticstoolbox/robot/Gripper.py index b4ff2fa9c..173cdde3b 100644 --- a/src/roboticstoolbox/robot/Gripper.py +++ b/src/roboticstoolbox/robot/Gripper.py @@ -237,4 +237,10 @@ def get_link_ets() -> Any: def get_link_scene_node() -> Any: return [link._T_reference for link in self.links] - Robot_link_T(get_link_ets(), get_link_scene_node(), self._q, q) + Robot_link_T( + get_link_ets(), + get_link_scene_node(), + self._q, + q, + _data=[link.ets.data for link in self.links], + ) diff --git a/tests/test_fknm_fallback.py b/tests/test_fknm_fallback.py index fd297e784..396f24721 100644 --- a/tests/test_fknm_fallback.py +++ b/tests/test_fknm_fallback.py @@ -1235,5 +1235,50 @@ def test_jacobe_c_faster_than_python(self): self._assert_c_faster(t_c, t_py) +class TestRobotLinkTReference(unittest.TestCase): + """ + The scene-node transforms that place a link's geometry follow the joints. + + Runs on whichever path is active. Without the compiled extension the + transforms used to be left at their initial values, so collision checks + and distance queries ran against geometry that did not move. + """ + + @staticmethod + def _world_pose(link): + # chain the local scene-node transforms up the kinematic tree + T = link._T_reference.copy() + while link.parent is not None: + link = link.parent + T = link._T_reference @ T + return T + + def test_geometry_follows_joints(self): + panda = rtb.models.Panda() + for q in (panda.qr, panda.qr + 0.4 * np.arange(1, 8) / 7): + panda.q = q + panda._update_link_tf() + for link in panda.links: + with self.subTest(link=link.name, q=q): + if len(link.ets.data) == 0: + # no elementary transforms: the identity + nt.assert_allclose(link._T_reference, np.eye(4)) + continue + nt.assert_allclose( + self._world_pose(link), + panda.fkine(q, end=link).A, + atol=1e-9, + ) + + def test_gripper_without_joints(self): + # an empty joint vector, and a link with no elementary transforms + ur5 = rtb.models.UR5() + ur5.q = np.zeros(ur5.n) + ur5._update_link_tf() + self.assertEqual(ur5.grippers[0].n, 0) + for link in ur5.links: + self.assertTrue(np.all(np.isfinite(link._T_reference))) + + if __name__ == "__main__": unittest.main()