feat(robot): add self-collision checks for non-adjacent links

docs/source/arm_superclass.rst

@@ -11,6 +11,29 @@ 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,
+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``
+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:

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,
@@ -1431,20 +1437,127 @@ 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, 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
+        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")
+            # 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()
+
+        # 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 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:
+                        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 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:
+            # 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()
@@ -1461,17 +1574,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)
 
-        :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 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)