hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Catch rare qhull errors

Browse Source
This commit is contained in:
Michel Breyer 2021-11-04 20:08:16 +01:00
parent 33d123e3ee
commit 3d7a8c8ba9
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/active_grasp/controller.py
View File

@ -161,12 +161,16 @@ class GraspController:
tip = grasp.pose.apply([0.0, 0.0, 0.05])
for label in range(labels.max() + 1):
segment = cloud.select_by_index(np.flatnonzero(labels == label))
hull = compute_convex_hull(segment)
name = f"object_{label}"
self.add_collision_mesh(name, hull)
dist = trimesh.proximity.closest_point_naive(hull, np.array([tip]))[1]
if dist < min_dist:
self.target_co_name, min_dist = name, dist
try:
hull = compute_convex_hull(segment)
name = f"object_{label}"
self.add_collision_mesh(name, hull)
dist = trimesh.proximity.closest_point_naive(hull, np.array([tip]))[1]
if dist < min_dist:
self.target_co_name, min_dist = name, dist
except:
# Qhull fails in some edge cases
pass
# Add collision object for the support
self.add_collision_mesh("support", compute_convex_hull(support_cloud))