check norm in inspection points divide by zero

act3-ace · Dec 16, 2024 · 20c8f45 · 20c8f45
1 parent 382a4d1
commit 20c8f45
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/safe_autonomy_simulation/sims/inspection/inspection_points.py b/safe_autonomy_simulation/sims/inspection/inspection_points.py
@@ -329,9 +329,15 @@ def _generate_points(
         else:
             point_weights = []
             for pos in point_positions:
-                dot_prod = np.dot(-self.priority_vector, pos) / (
-                    np.linalg.norm(-self.priority_vector) * np.linalg.norm(pos)
-                )
+                if (
+                    np.linalg.norm(-self.priority_vector) != 0
+                    and np.linalg.norm(pos) != 0
+                ):
+                    dot_prod = np.dot(-self.priority_vector, pos) / (
+                        np.linalg.norm(-self.priority_vector) * np.linalg.norm(pos)
+                    )
+                else:
+                    dot_prod = np.dot(-self.priority_vector, pos)
                 if dot_prod <= 0:
                     point_weights.append(np.arccos(dot_prod) / np.pi)
                 else: