add collision detection

kine_ctrl/rm75_kine_qp.py

@@ -20,8 +20,12 @@ class KinematicsSolver():
         unit: m, rad
         """
         print(f' ------------  the qp based kinematic initialising   -----------')
-        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
+        self.model, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
+        self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
+        self.geom_model.addAllCollisionPairs()
+        self.remove_adjacent_collision_pairs(verbose=True)
+        self.geom_data = pin.GeometryData(self.geom_model)
 
 
         self.cfg_j_limit()
@@ -335,12 +339,100 @@ class KinematicsSolver():
                 iter_count += 1
 
         if best_solution is not None:
-            # return best_solution, True, best_error, iter_count
-            return 0, best_solution.tolist()
+            collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
+
+            if collision is False:
+                # return best_solution, True, best_error, iter_count
+                return 0, best_solution.tolist()
+            else:
+                return -2, q[:7].copy().tolist()
         else:
             # return q[:7].copy(), False, error_norm, iter_count
             return -1, q[:7].copy().tolist()
 
+    def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ):
+        q = np.asarray(q, dtype=np.float64).reshape(-1)
+
+        if q.shape[0] != self.model.nq:
+            raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}")
+
+        # Update robot kinematics
+        pin.forwardKinematics(self.model, self.data, q)
+        pin.updateGeometryPlacements(
+            self.model,
+            self.data,
+            self.geom_model,
+            self.geom_data,
+            q
+        )
+
+        # Now compute collisions on the updated geometry model
+        collision = pin.computeCollisions(
+            self.geom_model,
+            self.geom_data,
+            stop_at_first_collision
+        )
+
+        if verbose:
+            print(f"the collision is {collision}\n")
+
+            for k, cr in enumerate(self.geom_data.collisionResults):
+                if cr.isCollision():
+                    cp = self.geom_model.collisionPairs[k]
+                    geom1 = self.geom_model.geometryObjects[cp.first]
+                    geom2 = self.geom_model.geometryObjects[cp.second]
+
+                    print(
+                        f"collision pair {k}: "
+                        f"{geom1.name} <--> {geom2.name}"
+                    )
+
+        return bool(collision)
+
+    def remove_adjacent_collision_pairs(self, verbose=True):
+        """
+        Remove collision pairs between same/adjacent parent joints.
+
+        This avoids false positives such as:
+            base_link_0 <--> link_1_0
+        """
+
+        pairs_to_remove = []
+
+        for pair_id, pair in enumerate(self.geom_model.collisionPairs):
+            geom1 = self.geom_model.geometryObjects[pair.first]
+            geom2 = self.geom_model.geometryObjects[pair.second]
+
+            j1 = geom1.parentJoint
+            j2 = geom2.parentJoint
+
+            # Same body or directly connected bodies
+            if j1 == j2 or abs(j1 - j2) <= 1:
+                pairs_to_remove.append(pair_id)
+
+                if verbose:
+                    print(
+                        "Removing adjacent pair:",
+                        pair_id,
+                        geom1.name,
+                        "<-->",
+                        geom2.name,
+                        "parentJoint:",
+                        j1,
+                        j2,
+                    )
+
+        for pair_id in reversed(pairs_to_remove):
+            self.geom_model.removeCollisionPair(
+                self.geom_model.collisionPairs[pair_id]
+            )
+
+        # Important: recreate geometry data after modifying pairs
+        self.geom_data = pin.GeometryData(self.geom_model)
+
+        if verbose:
+            print("Remaining collision pairs:", len(self.geom_model.collisionPairs))
+
     def quaternion_to_euler(self, q):
         """
         Convert quaternion to Euler angles (roll, pitch, yaw)