Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 30 additions & 2 deletions src/roboticstoolbox/ets/fknm.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
8 changes: 7 additions & 1 deletion src/roboticstoolbox/robot/BaseRobot.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
8 changes: 7 additions & 1 deletion src/roboticstoolbox/robot/Gripper.py
Original file line number Diff line number Diff line change
Expand Up @@ -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],
)
45 changes: 45 additions & 0 deletions tests/test_fknm_fallback.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Loading