Optionally check for collisions in Differential IK

.github/workflows/tests.yml

@@ -40,7 +40,7 @@ jobs:
           pytest-coverage-path: ./test/results/pytest-coverage.txt
           junitxml-path: ./test/results/test_results.xml
           pytest-xml-coverage-path: ./test/results/test_results_coverage.xml
-          coverage-path-prefix: src/pyroboplan/
+          coverage-path-prefix: src/
         # This will break on PRs from forks
         if: ${{ !github.event.pull_request.head.repo.fork }}
       - name: Create coverage badge

examples/example_differential_ik.py

@@ -1,3 +1,4 @@
+import hppfcl
 import pinocchio
 from pinocchio.visualize import MeshcatVisualizer
 import numpy as np
@@ -7,6 +8,52 @@
 from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions
 from pyroboplan.ik.nullspace_components import joint_limit_nullspace_component
 
+
+def prepare_scene(visual_model, collision_model):
+    """Helper function to create a collision scene for this example."""
+
+    # Add collision objects
+    obstacle_sphere_1 = pinocchio.GeometryObject(
+        "obstacle_sphere_1",
+        0,
+        hppfcl.Sphere(0.2),
+        pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])),
+    )
+    obstacle_sphere_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5])
+    visual_model.addGeometryObject(obstacle_sphere_1)
+    collision_model.addGeometryObject(obstacle_sphere_1)
+
+    obstacle_sphere_2 = pinocchio.GeometryObject(
+        "obstacle_sphere_2",
+        0,
+        hppfcl.Sphere(0.25),
+        pinocchio.SE3(np.eye(3), np.array([0.5, 0.5, 0.5])),
+    )
+    obstacle_sphere_2.meshColor = np.array([1.0, 1.0, 0.0, 0.5])
+    visual_model.addGeometryObject(obstacle_sphere_2)
+    collision_model.addGeometryObject(obstacle_sphere_2)
+
+    obstacle_box_1 = pinocchio.GeometryObject(
+        "obstacle_box_1",
+        0,
+        hppfcl.Box(0.25, 0.25, 0.25),
+        pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])),
+    )
+    obstacle_box_1.meshColor = np.array([1.0, 0.0, 0.0, 0.5])
+    visual_model.addGeometryObject(obstacle_box_1)
+    collision_model.addGeometryObject(obstacle_box_1)
+
+    obstacle_box_2 = pinocchio.GeometryObject(
+        "obstacle_box_2",
+        0,
+        hppfcl.Box(0.33, 0.33, 0.33),
+        pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])),
+    )
+    obstacle_box_2.meshColor = np.array([0.0, 0.0, 1.0, 0.5])
+    visual_model.addGeometryObject(obstacle_box_2)
+    collision_model.addGeometryObject(obstacle_box_2)
+
+
 if __name__ == "__main__":
     # Create models and data
     pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models")
@@ -18,14 +65,39 @@
     )
     data = model.createData()
 
+    # Comment out to exclude collision objects.
+    prepare_scene(visual_model, collision_model)
+
     # Initialize visualizer
     viz = MeshcatVisualizer(model, collision_model, visual_model, data=data)
     viz.initViewer(open=True)
     viz.loadViewerModel()
     np.set_printoptions(precision=3)
 
+    # Define the active collision pairs
+    collision_names = [
+        cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name
+    ]
+    obstacle_names = [
+        "obstacle_box_1",
+        "obstacle_box_2",
+        "obstacle_sphere_1",
+        "obstacle_sphere_2",
+    ]
+    for obstacle_name in obstacle_names:
+        for collision_name in collision_names:
+            collision_model.addCollisionPair(
+                pinocchio.CollisionPair(
+                    collision_model.getGeometryId(collision_name),
+                    collision_model.getGeometryId(obstacle_name),
+                )
+            )
+    collision_data = pinocchio.GeometryData(collision_model)
+
     # Set up the IK solver
-    ik = DifferentialIk(model, data, visualizer=viz, verbose=True)
+    ik = DifferentialIk(
+        model, data=data, collision_model=collision_model, visualizer=viz
+    )
     target_frame = "panda_hand"
     options = DifferentialIkOptions()
     nullspace_components = [
@@ -42,5 +114,6 @@
             init_state=init_state,
             options=options,
             nullspace_components=[],
+            verbose=True,
         )
         print(f"Solution configuration:\n{q_sol}\n")

src/pyroboplan/ik/differential_ik.py

@@ -4,7 +4,11 @@
 import pinocchio
 import time
 
-from ..core.utils import check_within_limits, get_random_state
+from ..core.utils import (
+    check_collisions_at_state,
+    check_within_limits,
+    get_random_state,
+)
 from ..visualization.meshcat_utils import visualize_frame
 
 VIZ_INITIAL_RENDER_TIME = 0.5
@@ -57,27 +61,29 @@ class DifferentialIk:
       * https://www.cs.cmu.edu/~15464-s13/lectures/lecture6/iksurvey.pdf
     """
 
-    def __init__(self, model, data=None, visualizer=None, verbose=False):
+    def __init__(self, model, collision_model=None, data=None, visualizer=None):
         """
         Creates an instance of a DifferentialIk solver.
 
         Parameters
         ----------
             model : `pinocchio.Model`
                 The model to use for this solver.
+            collision_model : `pinocchio.Model`, optional
+                The model to use for collision checking. If None, no collision checking takes place.
             data : `pinocchio.Data`, optional
                 The model data to use for this solver. If None, data is created automatically.
             visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`, optional
                 The visualizer to use for this solver.
-            verbose : bool, optional
-                If True, prints additional information to the console.
         """
         self.model = model
         if not data:
             data = model.createData()
         self.data = data
+
+        self.collision_model = collision_model
+
         self.visualizer = visualizer
-        self.verbose = verbose
 
     def solve(
         self,
@@ -86,6 +92,7 @@ def solve(
         init_state=None,
         options=DifferentialIkOptions(),
         nullspace_components=[],
+        verbose=False,
     ):
         """
         Solves an IK query.
@@ -103,6 +110,8 @@ def solve(
             nullspace_components : list[function], optional
                 An optional list of nullspace components to use when solving.
                 These components must take the form `lambda model, q: component(model, q, <other_args>)`.
+            verbose : bool, optional
+                If True, prints additional information to the console.
 
         Returns
         -------
@@ -142,14 +151,29 @@ def solve(
                     np.linalg.norm(error[:3]) < options.max_translation_error
                     and np.linalg.norm(error[3:]) < options.max_rotation_error
                 ):
-                    # Wrap to the range -/+ pi and check joint limits
+                    # Wrap to the range -/+ pi, and then check joint limits and collision.
                     q_cur = (q_cur + np.pi) % (2 * np.pi) - np.pi
                     if check_within_limits(self.model, q_cur):
-                        if self.verbose:
-                            print("Solved and within joint limits!")
-                        solved = True
+                        if self.collision_model is not None:
+                            if check_collisions_at_state(
+                                self.model, self.collision_model, q_cur
+                            ):
+                                if verbose:
+                                    print(
+                                        "Solved and within joint limits, but in collision."
+                                    )
+                            else:
+                                solved = True
+                                if verbose:
+                                    print(
+                                        "Solved, within joint limits, and collision-free!"
+                                    )
+                        else:
+                            solved = True
+                            if verbose:
+                                print("Solved and within joint limits!")
                     else:
-                        if self.verbose:
+                        if verbose:
                             print("Solved, but outside joint limits.")
                     break
 
@@ -194,13 +218,13 @@ def solve(
 
             # Check results at the end of this try
             if solved:
-                if self.verbose:
+                if verbose:
                     print(f"Solved in {n_tries+1} tries.")
                 break
             else:
                 q_cur = get_random_state(self.model)
                 n_tries += 1
-                if self.verbose:
+                if verbose:
                     print(f"Retry {n_tries}")
 
         # Check final results

test/ik/test_differential_ik.py

@@ -15,6 +15,39 @@ def load_panda_models():
     )
 
 
+def activate_panda_self_collision_pairs(collision_model):
+    collision_pair_names = [
+        ("panda_link0_0", "panda_link2_0"),
+        ("panda_link0_0", "panda_link3_0"),
+        ("panda_link0_0", "panda_link4_0"),
+        ("panda_link0_0", "panda_link5_0"),
+        ("panda_link0_0", "panda_link6_0"),
+        ("panda_link0_0", "panda_link7_0"),
+        ("panda_link1_0", "panda_link3_0"),
+        ("panda_link1_0", "panda_link4_0"),
+        ("panda_link1_0", "panda_link5_0"),
+        ("panda_link1_0", "panda_link6_0"),
+        ("panda_link1_0", "panda_link7_0"),
+        ("panda_link2_0", "panda_link4_0"),
+        ("panda_link2_0", "panda_link5_0"),
+        ("panda_link2_0", "panda_link6_0"),
+        ("panda_link2_0", "panda_link7_0"),
+        ("panda_link3_0", "panda_link5_0"),
+        ("panda_link3_0", "panda_link6_0"),
+        ("panda_link3_0", "panda_link7_0"),
+        ("panda_link4_0", "panda_link6_0"),
+        ("panda_link4_0", "panda_link7_0"),
+        ("panda_link5_0", "panda_link7_0"),
+    ]
+    for pair in collision_pair_names:
+        collision_model.addCollisionPair(
+            pinocchio.CollisionPair(
+                collision_model.getGeometryId(pair[0]),
+                collision_model.getGeometryId(pair[1]),
+            )
+        )
+
+
 def test_ik_solve_trivial_ik():
     model, _, _ = load_panda_models()
     data = model.createData()
@@ -29,7 +62,7 @@ def test_ik_solve_trivial_ik():
     target_tform = data.oMf[target_frame_id]
 
     # Solve
-    ik = DifferentialIk(model, data=None, visualizer=None, verbose=False)
+    ik = DifferentialIk(model, data=None, visualizer=None)
     options = DifferentialIkOptions()
     q_sol = ik.solve(
         target_frame,
@@ -59,7 +92,7 @@ def test_ik_solve_ik():
     target_tform.translation[2] = target_tform.translation[2] + offset
 
     # Solve it
-    ik = DifferentialIk(model, data=None, visualizer=None, verbose=False)
+    ik = DifferentialIk(model, data=None, visualizer=None)
     options = DifferentialIkOptions()
     q_sol = ik.solve(
         target_frame,
@@ -91,7 +124,7 @@ def test_ik_solve_impossible_ik():
     target_tform = pinocchio.SE3(R, T)
 
     # Solve
-    ik = DifferentialIk(model, data=None, visualizer=None, verbose=False)
+    ik = DifferentialIk(model, data=None, visualizer=None)
     options = DifferentialIkOptions()
     q_sol = ik.solve(
         target_frame,
@@ -102,3 +135,36 @@ def test_ik_solve_impossible_ik():
     )
 
     assert q_sol is None, "Solution should be impossible!"
+
+
+def test_ik_in_collision():
+    model, collision_model, _ = load_panda_models()
+    activate_panda_self_collision_pairs(collision_model)
+    target_frame = "panda_hand"
+
+    # Target is reachable, but in self-collision.
+    R = np.array(
+        [
+            [0.206636, -0.430153, 0.878789],
+            [-0.978337, -0.10235, 0.179946],
+            [0.0125395, -0.896935, -0.441984],
+        ],
+    )
+    T = np.array([0.155525, 0.0529695, 0.0259166])
+    target_tform = pinocchio.SE3(R, T)
+
+    # Solve
+    ik = DifferentialIk(
+        model, collision_model=collision_model, data=None, visualizer=None
+    )
+    options = DifferentialIkOptions()
+    q_sol = ik.solve(
+        target_frame,
+        target_tform,
+        init_state=None,
+        options=options,
+        nullspace_components=[],
+        verbose=False,
+    )
+
+    assert q_sol is None, "Solution should be in self-collision!"