From 73817dd2924df326fa7384be1c580f76d02cd049 Mon Sep 17 00:00:00 2001 From: Mykola Date: Fri, 16 Oct 2026 14:39:13 +0300 Subject: [PATCH 1/3] feat(robot): add self-collision checks for non-adjacent links Include gripper and nested collision geometry, reject models without collision shapes, and support iscollided(q, robot). Add documentation and 34 regression tests for issue #723. --- docs/source/arm_superclass.rst | 20 +++ src/roboticstoolbox/robot/Robot.py | 126 +++++++++++-- tests/test_self_collision.py | 280 +++++++++++++++++++++++++++++ 3 files changed, 416 insertions(+), 10 deletions(-) create mode 100644 tests/test_self_collision.py diff --git a/docs/source/arm_superclass.rst b/docs/source/arm_superclass.rst index b7a999e73..64551ec39 100644 --- a/docs/source/arm_superclass.rst +++ b/docs/source/arm_superclass.rst @@ -11,6 +11,26 @@ Robot The various models :ref:`E Models` all subclass this class. +Collision checking +^^^^^^^^^^^^^^^^^^ + +``robot.iscollided(q, shape)`` checks the robot against an external shape. +``robot.isselfcollided(q)`` checks its own non-adjacent links, including gripper +links; ``robot.iscollided(q, robot)`` is equivalent. Shapes on the same link and +direct parent-child link pairs are excluded. Links in separate branches are +checked, and touching counts as a collision. + +The checks use collision geometry, not visual geometry, and require the optional +``coal`` dependency. A model without collision geometry raises ``ValueError`` +for self-collision checking. Links without collision geometry are omitted, so +ensure the model represents all the parts that need checking. Many DH models +provide kinematics and dynamics without collision geometry. + +Omitting ``q`` from ``isselfcollided`` uses the robot's current configuration. +Grippers use their own current ``q``. The stored joint configurations are not +changed. ``skip=True`` reuses existing world transforms and should only be used +when the caller has already updated them. + .. automodule:: roboticstoolbox.robot.Robot :members: :undoc-members: diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index 338bbc8d2..dccbbf0f2 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -16,7 +16,13 @@ import numpy as np import spatialmath.base as smb -from spatialgeometry import CollisionShape, Cylinder, Shape +from spatialgeometry import ( + CollisionShape, + CollisionShapeGroup, + Cylinder, + SceneNode, + Shape, +) from spatialmath import ( SE2, SE3, @@ -1383,19 +1389,113 @@ def closest_point( return d, p1, p2 - def iscollided(self, q: Any, shape: Shape, skip: bool = False) -> bool: + def isselfcollided(self, q: ArrayLike | None = None, skip: bool = False) -> bool: + """ + Check for collisions between non-adjacent links of this robot + + :param q: joint configuration, defaults to the robot's current ``q`` + :type q: array_like(n) + :param skip: use the existing world transforms instead of updating them + from ``q``, defaults to False + :returns: True if any non-adjacent link pair is in collision + :raises ValueError: the model has no collision geometry, or ``q`` is + not a single finite joint configuration + + Checks the collision geometry of the robot and all its grippers. + Shapes attached to the same link, and links connected directly by a + parent-child relationship, are not tested against each other. Links + in separate branches are tested, including siblings and gripper links. + Touching counts as a collision, as in :meth:`iscollided`. + + Each gripper uses its own current joint configuration. The robot's + stored joint configuration is not changed. With ``skip=True``, ``q`` + is ignored and the caller must have updated all world transforms. + + Collision checking requires the optional ``coal`` dependency. A model + with only visual geometry cannot be checked; collision geometry must + be supplied explicitly. Only pairs with geometry are checked, so a + False result does not establish clearance for links without geometry. + + :seealso: :meth:`iscollided` :meth:`closest_point` + """ + + def collision_shapes(node: SceneNode) -> list[CollisionShape]: + # Groups may be nested or empty; only concrete collision shapes + # provide geometry that the collision engine can test. + if isinstance(node, CollisionShape) and not isinstance( + node, CollisionShapeGroup + ): + return [node] + return [ + shape + for child in node.scene_children + for shape in collision_shapes(child) + ] + + # Gripper links are stored separately from the main kinematic chain. + links = [*self.links, *(link for g in self.grippers for link in g.links)] + geometries = [(link, collision_shapes(link.collision)) for link in links] + geometries = [(link, shapes) for link, shapes in geometries if shapes] + if not geometries: + raise ValueError(f"Robot {self.name!r} has no collision geometry") + + if not skip: + configuration = self.q if q is None else getvector(q, self.n) + if configuration.ndim != 1 or not np.all(np.isfinite(configuration)): + raise ValueError("q must be a single finite joint configuration") + # The scene update path relies on the optional C extension. Use FK + # directly so collision checking also works with the Python facade, + # and keep each collision group's local placement intact. + frames = self.fkine_all(configuration) + + def update_collision(node: SceneNode, parent: SE3) -> None: + pose = parent * SE3(node.T) + # Coal reads the cached world pose. Keep the local transform + # unchanged so repeated queries do not accumulate transforms. + node._wT[:] = pose.A + node._wq[:] = smb.r2q(pose.R, order="xyzs") + for child in node.scene_children: + update_collision(child, pose) + + for link, _ in geometries: + update_collision(link.collision, frames[link.number]) + + for i, (link, shapes) in enumerate(geometries): + # Visit each unordered pair once. Adjacency is determined by the + # link tree, since consecutive list entries can belong to branches. + for other, other_shapes in geometries[i + 1 :]: + if link.parent is other or other.parent is link: + continue + for shape in shapes: + for other_shape in other_shapes: + if shape.iscollided(other_shape): + return True + return False + + def iscollided( + self, q: ArrayLike | None, shape: Shape | Robot, skip: bool = False + ) -> bool: """ Check if the robot is in collision with a shape - :param shape: The shape to compare distance to + :param q: joint configuration, or None for the current configuration + :param shape: the external shape to check, or this robot itself for + self-collision checking :param skip: Skip setting all shape transforms based on q, use this - option if using this method in conjuction with Swift to save time + option if using this method in conjunction with Swift to save time :returns: True if shapes have collided - ``iscollided(shape)`` checks if this robot and shape have collided + ``robot.iscollided(q, shape)`` checks for collision with an external + shape. ``robot.iscollided(q, robot)`` calls :meth:`isselfcollided`. + :seealso: :meth:`isselfcollided` :meth:`closest_point` """ + if isinstance(shape, Robot): + if shape is not self: + raise TypeError("shape must be an external Shape or this robot itself") + return self.isselfcollided(q, skip=skip) + if not skip: self._update_link_tf(q) self.update() @@ -1413,17 +1513,23 @@ def iscollided(self, q: Any, shape: Shape, skip: bool = False) -> bool: return False - def collided(self, q: Any, shape: Shape, skip: bool = False) -> bool: + def collided( + self, q: ArrayLike | None, shape: Shape | Robot, skip: bool = False + ) -> bool: """ - Check if the robot is in collision with a shape + Deprecated alias for :meth:`iscollided` - :param shape: The shape to compare distance to + :param q: joint configuration, or None for the current configuration + :param shape: the external shape to check, or this robot itself for + self-collision checking :param skip: Skip setting all shape transforms based on q, use this - option if using this method in conjuction with Swift to save time + option if using this method in conjunction with Swift to save time :returns: True if shapes have collided - ``collided(shape)`` checks if this robot and shape have collided + ``robot.collided(q, shape)`` issues a warning and returns the result + of :meth:`iscollided`. + :seealso: :meth:`iscollided` :meth:`isselfcollided` """ warn("method collided is deprecated, use iscollided instead", FutureWarning) diff --git a/tests/test_self_collision.py b/tests/test_self_collision.py new file mode 100644 index 000000000..df403a416 --- /dev/null +++ b/tests/test_self_collision.py @@ -0,0 +1,280 @@ +""" +Tests for robot self-collision checking. + +Topology tests check pair selection independently of the collision engine. +Geometry tests use Coal with simple shapes whose intersections are known +analytically, including nested groups, grippers and repeated pose updates. +""" + +from unittest.mock import patch + +import numpy as np +import pytest +from spatialgeometry import CollisionShapeGroup, Cuboid, SceneGroup, Sphere +from spatialmath import SE3 + +from roboticstoolbox import ET, DHRobot, Link, RevoluteDH, Robot, models +from roboticstoolbox.tools.types import ArrayLike +from tests import skip_no_collision_checking + + +def chain() -> Robot: + """ + Construct a two-joint chain with collision geometry on its base and tip + + :returns: robot with two unit-length segments and spherical geometry + """ + # The tip centre moves from x=2 to the base centre as q goes from 0 to pi. + base = Link(ET.Rz(), name="base", collision=[Sphere(0.2)]) + middle = Link(ET.tx(1) * ET.Rz(), parent=base, name="middle") + tip = Link(ET.tx(1), parent=middle, name="tip", collision=[Sphere(0.2)]) + return Robot([base, middle, tip]) + + +def test_model_without_collision_geometry_raises() -> None: + robot = Robot([Link(ET.Rz(), geometry=[Sphere(1)])], name="visual only") + with pytest.raises(ValueError, match="no collision geometry"): + robot.isselfcollided([0]) + with pytest.raises(ValueError, match="no collision geometry"): + robot.iscollided([0], robot) + + +def test_reported_dh_panda_example_raises_clear_error() -> None: + robot = models.DH.Panda() + with pytest.raises(ValueError, match="no collision geometry"): + robot.iscollided(robot.q, robot) + + +@pytest.mark.parametrize("depth", [1, 3]) +@pytest.mark.parametrize("group_type", [CollisionShapeGroup, SceneGroup]) +def test_empty_nested_groups_are_not_collision_geometry( + depth: int, group_type: type[CollisionShapeGroup] | type[SceneGroup] +) -> None: + group = group_type() + for _ in range(depth): + group = group_type([group]) + robot = Robot([Link(ET.Rz(), collision=group)]) + with pytest.raises(ValueError, match="no collision geometry"): + robot.isselfcollided([0]) + + +def test_another_robot_is_not_accepted_as_an_external_shape() -> None: + robot = chain() + with pytest.raises(TypeError, match="external Shape or this robot"): + robot.iscollided([0, 0], chain()) + + +def test_collision_geometry_added_after_construction_is_used() -> None: + robot = Robot([Link(ET.Rz())]) + robot.links[0].collision.append(Sphere(1)) + assert robot.isselfcollided([0]) is False + + +def test_geometry_on_one_link_cannot_collide_with_itself() -> None: + robot = Robot([Link(ET.Rz(), collision=[Sphere(1), Sphere(1)])]) + with patch.object(Sphere, "iscollided") as collided: + assert robot.isselfcollided([0]) is False + collided.assert_not_called() + + +def test_adjacent_links_are_not_checked() -> None: + base = Link(ET.Rz(), collision=[Sphere(1)]) + tip = Link(ET.Rz(), parent=base, collision=[Sphere(1)]) + robot = Robot([base, tip]) + with patch.object(Sphere, "iscollided") as collided: + assert robot.isselfcollided([0, 0]) is False + collided.assert_not_called() + + +def test_each_non_adjacent_shape_pair_is_checked_once() -> None: + robot = chain() + robot.links[0].collision.append(Sphere(0.1)) + robot.links[-1].collision.append(Sphere(0.1)) + with patch.object(Sphere, "iscollided", return_value=False) as collided: + assert robot.isselfcollided([0, 0]) is False + assert collided.call_count == 4 + + +def test_first_collision_stops_the_search() -> None: + robot = chain() + robot.links[0].collision.append(Sphere(0.1)) + with patch.object(Sphere, "iscollided", return_value=True) as collided: + assert robot.isselfcollided([0, np.pi]) is True + collided.assert_called_once() + + +def test_skip_reuses_transforms() -> None: + robot = chain() + with ( + patch.object(robot, "_update_link_tf") as update_links, + patch.object(robot, "update") as update, + patch.object(robot, "fkine_all") as fkine_all, + patch.object(Sphere, "iscollided", return_value=False), + ): + assert robot.isselfcollided([0, 0], skip=True) is False + update_links.assert_not_called() + update.assert_not_called() + fkine_all.assert_not_called() + + +def test_self_argument_delegates_without_changing_external_shape_api() -> None: + robot = chain() + with patch.object(robot, "isselfcollided", return_value=True) as collided: + assert robot.iscollided([0, np.pi], robot, skip=True) is True + collided.assert_called_once_with([0, np.pi], skip=True) + + +@skip_no_collision_checking +@pytest.mark.parametrize("q,expected", [([0, 0], False), ([0, np.pi], True)]) +def test_real_spheres_at_requested_configuration(q: ArrayLike, expected: bool) -> None: + robot = chain() + robot.q = [0.3, 0.4] + assert robot.isselfcollided(q) is expected + np.testing.assert_array_equal(robot.q, [0.3, 0.4]) + + +@skip_no_collision_checking +def test_reported_urdf_panda_example_accepts_self() -> None: + robot = models.Panda() + before = robot.q.copy() + assert isinstance(robot.iscollided(robot.q, robot), bool) + np.testing.assert_array_equal(robot.q, before) + + +@skip_no_collision_checking +def test_touching_non_adjacent_shapes_count_as_collision() -> None: + robot = chain() + robot.links[0].collision[0].radius = 1 + robot.links[-1].collision[0].radius = 1 + assert robot.isselfcollided([0, 0]) is True + + +@skip_no_collision_checking +def test_later_shape_on_link_can_be_the_only_collision() -> None: + robot = chain() + robot.links[-1].collision.append(Sphere(0.2, pose=SE3.Tx(-2))) + assert robot.isselfcollided([0, 0]) is True + + +@skip_no_collision_checking +def test_nested_collision_group_preserves_local_transforms() -> None: + robot = chain() + group = CollisionShapeGroup([Sphere(0.2)]) + group.T = SE3.Tx(-2) + robot.links[-1].collision = [group] + before = group.T.copy() + assert robot.isselfcollided([0, 0]) is True + assert robot.isselfcollided([0, np.pi]) is False + np.testing.assert_array_equal(group.T, before) + + +@skip_no_collision_checking +@pytest.mark.parametrize("group_type", [CollisionShapeGroup, SceneGroup]) +def test_rotated_cuboid_in_nested_group( + group_type: type[CollisionShapeGroup] | type[SceneGroup], +) -> None: + base = Link(ET.Rz(), collision=[Sphere(0.1, pose=SE3.Tx(1))]) + middle = Link(ET.tx(0), parent=base) + group = group_type([Cuboid([3, 0.1, 0.1])]) + group.T = SE3.Rz(np.pi / 2) + tip = Link(ET.Rz(), parent=middle, collision=[CollisionShapeGroup(), group]) + robot = Robot([base, middle, tip]) + # The long axis starts along y, clear of the sphere at x=1. Rotating + # the joint by pi/2 brings that axis onto x and intersects the sphere. + assert robot.isselfcollided([0, 0]) is False + assert robot.isselfcollided([0, np.pi / 2]) is True + assert robot.isselfcollided([0, 0]) is False + + +@skip_no_collision_checking +def test_collision_between_separate_grippers() -> None: + root = Link(ET.Rz(), name="root") + first = Link(ET.tx(1) * ET.tx(), parent=root, name="first", collision=[Sphere(0.2)]) + second = Link( + ET.tx(2) * ET.tx(), parent=root, name="second", collision=[Sphere(0.2)] + ) + robot = Robot([root, first, second], gripper_links=[first, second]) + assert robot.isselfcollided([0]) is False + robot.grippers[1].q = [-1] + assert robot.isselfcollided([0]) is True + np.testing.assert_array_equal(robot.grippers[0].q, [0]) + np.testing.assert_array_equal(robot.grippers[1].q, [-1]) + + +@skip_no_collision_checking +def test_default_configuration_and_repeated_queries() -> None: + robot = chain() + robot.q = [0, np.pi] + assert robot.isselfcollided() is True + assert robot.isselfcollided([0, 0]) is False + assert robot.isselfcollided([0, np.pi]) is True + assert robot.isselfcollided(skip=True) is True + + +@skip_no_collision_checking +def test_external_shape_queries_and_deprecated_alias() -> None: + robot = chain() + assert robot.iscollided([0, 0], Sphere(0.1)) is True + assert robot.iscollided([0, 0], Sphere(0.1, pose=SE3.Tx(5))) is False + assert robot.iscollided([0, np.pi], robot) is True + with pytest.warns(FutureWarning, match="deprecated"): + assert robot.collided([0, np.pi], robot) is True + + +@skip_no_collision_checking +def test_base_and_shape_local_transforms_are_applied() -> None: + robot = chain() + robot.base = SE3(3, -2, 1) * SE3.Ry(0.4) + robot.links[-1].collision[0].T = SE3.Tz(1).A + assert robot.isselfcollided([0, np.pi]) is False + robot.links[-1].collision[0].T = SE3().A + assert robot.isselfcollided([0, np.pi]) is True + + +@skip_no_collision_checking +def test_sibling_links_are_not_treated_as_adjacent() -> None: + root = Link(ET.Rz(), name="root") + left = Link(ET.tx(1), parent=root, name="left", collision=[Sphere(0.2)]) + right = Link( + ET.ty(0.1) * ET.Rz(), + parent=root, + name="right", + collision=[Sphere(0.2, pose=SE3.Tx(1))], + ) + robot = Robot([root, left, right]) + assert robot.isselfcollided([0, 0]) is True + assert robot.isselfcollided([0, np.pi]) is False + + +@skip_no_collision_checking +def test_gripper_links_use_their_own_configuration() -> None: + base = Link(ET.Rz(), name="base", collision=[Sphere(0.2)]) + wrist = Link(ET.tx(1), parent=base, name="wrist") + finger = Link( + ET.tx(1) * ET.tx(), parent=wrist, name="finger", collision=[Sphere(0.2)] + ) + robot = Robot([base, wrist, finger], gripper_links=finger) + assert robot.isselfcollided([0]) is False + robot.grippers[0].q = [-2] + assert robot.isselfcollided([0]) is True + + +@skip_no_collision_checking +def test_dh_robot_with_collision_geometry() -> None: + robot = DHRobot( + [ + RevoluteDH(a=1, collision=[Sphere(0.2, pose=SE3.Tx(-1))]), + RevoluteDH(a=1), + RevoluteDH(a=0, collision=[Sphere(0.2)]), + ] + ) + assert robot.isselfcollided([0, 0, 0]) is False + assert robot.isselfcollided([0, np.pi, 0]) is True + + +@pytest.mark.parametrize( + "q", [[0], [0, 0, 0], [[0, 0], [0, 0]], [0, np.nan], [np.inf, 0]] +) +def test_configuration_must_be_one_joint_vector(q: ArrayLike) -> None: + with pytest.raises(ValueError): + chain().isselfcollided(q) From 6b07e6c008f5994e9d8122bfb10bab909c00c682 Mon Sep 17 00:00:00 2001 From: Mykola Date: Fri, 16 Oct 2026 16:06:56 +0300 Subject: [PATCH 2/3] fix: normalise joint vectors for external collision checks --- src/roboticstoolbox/robot/Robot.py | 3 +++ tests/test_self_collision.py | 20 +++++++++++++++++--- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index dccbbf0f2..8a2ed19f7 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1497,6 +1497,9 @@ def iscollided( return self.isselfcollided(q, skip=skip) if not skip: + # The compiled scene update requires a floating-point joint vector. + if q is not None: + q = getvector(q, self.n) self._update_link_tf(q) self.update() shape.update() diff --git a/tests/test_self_collision.py b/tests/test_self_collision.py index df403a416..61b0acad9 100644 --- a/tests/test_self_collision.py +++ b/tests/test_self_collision.py @@ -212,10 +212,24 @@ def test_default_configuration_and_repeated_queries() -> None: @skip_no_collision_checking -def test_external_shape_queries_and_deprecated_alias() -> None: +@pytest.mark.parametrize( + "q", + [ + [0, 0], + (0, 0), + np.zeros(2, dtype=int), + np.zeros(2, dtype=np.float32), + np.zeros((2, 1)), + None, + ], + ids=["list", "tuple", "integer-array", "float32-array", "column-vector", "current"], +) +def test_external_shape_queries_and_deprecated_alias(q: ArrayLike | None) -> None: robot = chain() - assert robot.iscollided([0, 0], Sphere(0.1)) is True - assert robot.iscollided([0, 0], Sphere(0.1, pose=SE3.Tx(5))) is False + assert robot.iscollided(q, Sphere(0.1)) is True + assert robot.iscollided(q, Sphere(0.1, pose=SE3.Tx(5))) is False + with pytest.warns(FutureWarning, match="deprecated"): + assert robot.collided(q, Sphere(0.1)) is True assert robot.iscollided([0, np.pi], robot) is True with pytest.warns(FutureWarning, match="deprecated"): assert robot.collided([0, np.pi], robot) is True From 953b16c05c0880d6360d8f898171c79cb63b3f35 Mon Sep 17 00:00:00 2001 From: Mykola Date: Wed, 21 Oct 2026 22:05:13 +0300 Subject: [PATCH 3/3] fix(robot): respect collision neighbours and public scene updates Co-Authored-By: Codex (GPT-6) --- docs/source/arm_superclass.rst | 9 ++-- src/roboticstoolbox/robot/Robot.py | 46 ++++++++++++-------- tests/test_self_collision.py | 67 +++++++++++++++++++++++++++--- 3 files changed, 96 insertions(+), 26 deletions(-) diff --git a/docs/source/arm_superclass.rst b/docs/source/arm_superclass.rst index 64551ec39..7970a3291 100644 --- a/docs/source/arm_superclass.rst +++ b/docs/source/arm_superclass.rst @@ -16,9 +16,12 @@ Collision checking ``robot.iscollided(q, shape)`` checks the robot against an external shape. ``robot.isselfcollided(q)`` checks its own non-adjacent links, including gripper -links; ``robot.iscollided(q, robot)`` is equivalent. Shapes on the same link and -direct parent-child link pairs are excluded. Links in separate branches are -checked, and touching counts as a collision. +links; ``robot.iscollided(q, robot)`` is equivalent. Shapes on the same link, +neighbouring links, and links within the same gripper are excluded. Links without +collision geometry are transparent when determining neighbours, so each link's +nearest ancestor with collision geometry is excluded. Links in separate branches +are checked unless they belong to the same gripper, and touching counts as a +collision for the pairs that are checked. The checks use collision geometry, not visual geometry, and require the optional ``coal`` dependency. A model without collision geometry raises ``ValueError`` diff --git a/src/roboticstoolbox/robot/Robot.py b/src/roboticstoolbox/robot/Robot.py index af36e245a..4081c727b 100644 --- a/src/roboticstoolbox/robot/Robot.py +++ b/src/roboticstoolbox/robot/Robot.py @@ -1450,9 +1450,12 @@ def isselfcollided(self, q: ArrayLike | None = None, skip: bool = False) -> bool not a single finite joint configuration Checks the collision geometry of the robot and all its grippers. - Shapes attached to the same link, and links connected directly by a - parent-child relationship, are not tested against each other. Links - in separate branches are tested, including siblings and gripper links. + Shapes attached to the same link, neighbouring links, and links in + the same gripper are not tested against each other. Links without + collision geometry are transparent when determining neighbours: a + link's nearest ancestor with collision geometry is its neighbour. + Links in separate branches are tested, including siblings outside a + shared gripper and links belonging to different grippers. Touching counts as a collision, as in :meth:`iscollided`. Each gripper uses its own current joint configuration. The robot's @@ -1491,28 +1494,35 @@ def collision_shapes(node: SceneNode) -> list[CollisionShape]: configuration = self.q if q is None else getvector(q, self.n) if configuration.ndim != 1 or not np.all(np.isfinite(configuration)): raise ValueError("q must be a single finite joint configuration") - # The scene update path relies on the optional C extension. Use FK - # directly so collision checking also works with the Python facade, - # and keep each collision group's local placement intact. + # FK works with both the compiled extension and the Python facade. + # Set local link poses through the public scene API, then let + # spatialgeometry update world poses and collision orientations. frames = self.fkine_all(configuration) + for link in links: + parent = frames[link.parent.number] if link.parent else self.base + link.T = parent.inv() * frames[link.number] + self.update() - def update_collision(node: SceneNode, parent: SE3) -> None: - pose = parent * SE3(node.T) - # Coal reads the cached world pose. Keep the local transform - # unchanged so repeated queries do not accumulate transforms. - node._wT[:] = pose.A - node._wq[:] = smb.r2q(pose.R, order="xyzs") - for child in node.scene_children: - update_collision(child, pose) - - for link, _ in geometries: - update_collision(link.collision, frames[link.number]) + # Empty spacer links do not separate neighbouring collision bodies, + # for example Panda's link7 -> link8 (no geometry) -> hand. + collision_links = {link for link, _ in geometries} + neighbours = {} + for link, _ in geometries: + ancestor = link.parent + while ancestor is not None and ancestor not in collision_links: + ancestor = ancestor.parent + neighbours[link] = ancestor + grippers = {link: g for g in self.grippers for link in g.links} for i, (link, shapes) in enumerate(geometries): # Visit each unordered pair once. Adjacency is determined by the # link tree, since consecutive list entries can belong to branches. for other, other_shapes in geometries[i + 1 :]: - if link.parent is other or other.parent is link: + if neighbours[link] is other or neighbours[other] is link: + continue + # Finger contact is normal within a gripper; collisions with + # the arm or another gripper must still be checked. + if link in grippers and grippers[link] is grippers.get(other): continue for shape in shapes: for other_shape in other_shapes: diff --git a/tests/test_self_collision.py b/tests/test_self_collision.py index 61b0acad9..6faa234ac 100644 --- a/tests/test_self_collision.py +++ b/tests/test_self_collision.py @@ -20,13 +20,19 @@ def chain() -> Robot: """ - Construct a two-joint chain with collision geometry on its base and tip + Construct a two-joint chain with collision geometry on all three links :returns: robot with two unit-length segments and spherical geometry """ # The tip centre moves from x=2 to the base centre as q goes from 0 to pi. base = Link(ET.Rz(), name="base", collision=[Sphere(0.2)]) - middle = Link(ET.tx(1) * ET.Rz(), parent=base, name="middle") + # A distant middle body keeps base and tip non-adjacent without colliding. + middle = Link( + ET.tx(1) * ET.Rz(), + parent=base, + name="middle", + collision=[Sphere(0.1, pose=SE3.Tz(10))], + ) tip = Link(ET.tx(1), parent=middle, name="tip", collision=[Sphere(0.2)]) return Robot([base, middle, tip]) @@ -86,6 +92,37 @@ def test_adjacent_links_are_not_checked() -> None: collided.assert_not_called() +@pytest.mark.parametrize("depth", [1, 3]) +def test_empty_spacer_links_are_transparent_to_adjacency(depth: int) -> None: + base = Link(ET.Rz(), name="base", collision=[Sphere(1)]) + links = [base] + for i in range(depth): + links.append( + Link( + ET.tx(0), + parent=links[-1], + name=f"spacer{i}", + collision=[CollisionShapeGroup([CollisionShapeGroup()])], + ) + ) + links.append(Link(ET.tx(0), parent=links[-1], name="tip", collision=[Sphere(1)])) + robot = Robot(links) + with patch.object(Sphere, "iscollided") as collided: + assert robot.isselfcollided([0]) is False + collided.assert_not_called() + + +def test_links_in_same_gripper_are_not_checked() -> None: + root = Link(ET.Rz(), name="root") + hand = Link(ET.tx(0), parent=root, name="hand", collision=[Sphere(1)]) + left = Link(ET.tx(), parent=hand, name="left", collision=[Sphere(1)]) + right = Link(ET.ty(), parent=hand, name="right", collision=[Sphere(1)]) + robot = Robot([root, hand, left, right], gripper_links=hand) + with patch.object(Sphere, "iscollided") as collided: + assert robot.isselfcollided([0]) is False + collided.assert_not_called() + + def test_each_non_adjacent_shape_pair_is_checked_once() -> None: robot = chain() robot.links[0].collision.append(Sphere(0.1)) @@ -141,6 +178,19 @@ def test_reported_urdf_panda_example_accepts_self() -> None: np.testing.assert_array_equal(robot.q, before) +@skip_no_collision_checking +@pytest.mark.parametrize("model", [models.Panda, models.Frankie]) +def test_panda_and_frankie_ready_pose_is_collision_free(model: type[Robot]) -> None: + robot = model() + before = robot.q.copy() + gripper_configurations = [g.q.copy() for g in robot.grippers] + assert robot.isselfcollided(robot.qr) is False + assert robot.iscollided(robot.qr, robot) is False + np.testing.assert_array_equal(robot.q, before) + for gripper, configuration in zip(robot.grippers, gripper_configurations): + np.testing.assert_array_equal(gripper.q, configuration) + + @skip_no_collision_checking def test_touching_non_adjacent_shapes_count_as_collision() -> None: robot = chain() @@ -174,7 +224,7 @@ def test_rotated_cuboid_in_nested_group( group_type: type[CollisionShapeGroup] | type[SceneGroup], ) -> None: base = Link(ET.Rz(), collision=[Sphere(0.1, pose=SE3.Tx(1))]) - middle = Link(ET.tx(0), parent=base) + middle = Link(ET.tx(0), parent=base, collision=[Sphere(0.1, pose=SE3.Tz(10))]) group = group_type([Cuboid([3, 0.1, 0.1])]) group.T = SE3.Rz(np.pi / 2) tip = Link(ET.Rz(), parent=middle, collision=[CollisionShapeGroup(), group]) @@ -207,7 +257,12 @@ def test_default_configuration_and_repeated_queries() -> None: robot.q = [0, np.pi] assert robot.isselfcollided() is True assert robot.isselfcollided([0, 0]) is False + # A public scene update must retain the requested configuration, rather + # than overwrite collision poses with stale local link transforms. + robot.update() + assert robot.isselfcollided(skip=True) is False assert robot.isselfcollided([0, np.pi]) is True + robot.update() assert robot.isselfcollided(skip=True) is True @@ -263,7 +318,9 @@ def test_sibling_links_are_not_treated_as_adjacent() -> None: @skip_no_collision_checking def test_gripper_links_use_their_own_configuration() -> None: base = Link(ET.Rz(), name="base", collision=[Sphere(0.2)]) - wrist = Link(ET.tx(1), parent=base, name="wrist") + wrist = Link( + ET.tx(1), parent=base, name="wrist", collision=[Sphere(0.1, pose=SE3.Tz(10))] + ) finger = Link( ET.tx(1) * ET.tx(), parent=wrist, name="finger", collision=[Sphere(0.2)] ) @@ -278,7 +335,7 @@ def test_dh_robot_with_collision_geometry() -> None: robot = DHRobot( [ RevoluteDH(a=1, collision=[Sphere(0.2, pose=SE3.Tx(-1))]), - RevoluteDH(a=1), + RevoluteDH(a=1, collision=[Sphere(0.1, pose=SE3.Tz(10))]), RevoluteDH(a=0, collision=[Sphere(0.2)]), ] )