update

Author: Kashu7100

genesis/utils/path_planing.py

@@ -89,40 +89,24 @@ def format_input(self, qpos_cur, qpos_goal, qpos_start, envs_idx):
         return qpos_cur, qpos_goal, qpos_start, envs_idx
 
     def get_exclude_geom_pairs(self, qpos_goal, qpos_start, envs_idx):
-        if self._solver.n_envs > 0:
-            self._entity.set_qpos(qpos_goal, envs_idx=envs_idx)
-        else:
-            self._entity.set_qpos(qpos_goal[0])
-        self._solver._kernel_detect_collision()
-        scene_contact_info = self._entity.get_contacts()
-        geom_a_goal = scene_contact_info["geom_a"]
-        geom_b_goal = scene_contact_info["geom_b"]
-        if self._solver.n_envs > 0:
-            valid_mask = scene_contact_info["valid_mask"]
-            geom_a_goal = geom_a_goal[valid_mask]
-            geom_b_goal = geom_b_goal[valid_mask]
-
-        if self._solver.n_envs > 0:
-            self._entity.set_qpos(qpos_start, envs_idx=envs_idx)
-        else:
-            self._entity.set_qpos(qpos_start[0])
-        self._solver._kernel_detect_collision()
-        scene_contact_info = self._entity.get_contacts()
-        geom_a_start = scene_contact_info["geom_a"]
-        geom_b_start = scene_contact_info["geom_b"]
-        if self._solver.n_envs > 0:
-            valid_mask = scene_contact_info["valid_mask"]
-            geom_a_start = geom_a_start[valid_mask]
-            geom_b_start = geom_b_start[valid_mask]
+        collision_pairs = []
+        for qpos in [qpos_start, qpos_goal]:
+            if self._solver.n_envs > 0:
+                self._entity.set_qpos(qpos, envs_idx=envs_idx)
+            else:
+                self._entity.set_qpos(qpos[0])
+            self._solver._kernel_detect_collision()
+            scene_contact_info = self._entity.get_contacts()
+            geom_a = scene_contact_info["geom_a"]
+            geom_b = scene_contact_info["geom_b"]
+            if self._solver.n_envs > 0:
+                valid_mask = scene_contact_info["valid_mask"]
+                geom_a = geom_a[valid_mask]
+                geom_b = geom_b[valid_mask]
+            collision_pairs.append(torch.stack((geom_a, geom_b), dim=1))
 
-        # NOTE: we will reduce the contacts in batch dim assuming internal geom collisions are the same for a robot
         unique_pairs = torch.unique(
-            torch.cat(
-                [
-                    torch.stack((geom_a_start, geom_b_start), dim=1),
-                    torch.stack((geom_a_goal, geom_b_goal), dim=1),
-                ]
-            ),
+            torch.cat(collision_pairs, dim=0),
             dim=0,
         )  # N', 2
         return unique_pairs

tests/test_rigid_physics.py

@@ -1571,6 +1571,8 @@ def test_path_planning_avoidance(show_viewer):
     assert_allclose(free_path[0], 0, tol=gs.EPS)
     assert_allclose(free_path[-1], qpos, tol=gs.EPS)
 
+    qpos = franka.inverse_kinematics(hand, pos=hand_pos_ref, quat=hand_quat_ref)
+    qpos[-2:] = 0.04
     avoidance_path = franka.plan_path(
         qpos_goal=qpos,
         num_waypoints=300,