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

Minor

Browse Source
This commit is contained in:
Michel Breyer 2021-10-12 17:57:09 +02:00
parent a4e170966a
commit 505b768cce
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/active_grasp/controller.py
View File

@ -138,22 +138,26 @@ class GraspController:
# Segment support plane, cluster, and create collision objects from convex hulls # Segment support plane, cluster, and create collision objects from convex hulls
cloud = self.policy.tsdf.get_scene_cloud() cloud = self.policy.tsdf.get_scene_cloud()
cloud = cloud.transform(self.policy.T_base_task.as_matrix()) cloud = cloud.transform(self.policy.T_base_task.as_matrix())
plane_model, inliers = cloud.segment_plane(0.01, 3, 1000) _, inliers = cloud.segment_plane(0.01, 3, 1000)
cloud = cloud.select_by_index(inliers, invert=True) cloud = cloud.select_by_index(inliers, invert=True)
labels = np.array(cloud.cluster_dbscan(eps=0.02, min_points=10)) labels = np.array(cloud.cluster_dbscan(eps=0.02, min_points=10))
self.target_co_name, min_dist = None, np.inf self.target_co_name, min_dist = None, np.inf
tip = grasp.pose.apply([0.0, 0.0, 0.05]) tip = grasp.pose.apply([0.0, 0.0, 0.05])
for l in range(labels.max() + 1): for l in range(labels.max() + 1):
segment = cloud.select_by_index(np.flatnonzero(labels == l)) segment = cloud.select_by_index(np.flatnonzero(labels == l))
hull = trimesh.points.PointCloud(np.asarray(segment.points)).convex_hull hull = compute_convex_hull(segment)
name, frame, pose = f"object_{l}", self.base_frame, Transform.identity() name = f"object_{l}"
co = create_collision_object_from_mesh(name, frame, pose, hull) self.add_collision_mesh(name, hull)
self.moveit.scene.add_object(co)
dist = trimesh.proximity.closest_point_naive(hull, np.array([tip]))[1] dist = trimesh.proximity.closest_point_naive(hull, np.array([tip]))[1]
if dist < min_dist: if dist < min_dist:
self.target_co_name, min_dist = name, dist self.target_co_name, min_dist = name, dist
rospy.sleep(1.0) rospy.sleep(1.0)
def add_collision_mesh(self, name, mesh):
frame, pose = self.base_frame, Transform.identity()
co = create_collision_object_from_mesh(name, frame, pose, mesh)
self.moveit.scene.add_object(co)
def execute_grasp(self, grasp): def execute_grasp(self, grasp):
T_base_grasp = self.postprocess(grasp.pose) T_base_grasp = self.postprocess(grasp.pose)
self.gripper.move(0.08) self.gripper.move(0.08)
@ -186,6 +190,10 @@ class GraspController:
return info return info
def compute_convex_hull(cloud):
return trimesh.points.PointCloud(np.asarray(cloud.points)).convex_hull
class ViewHalfSphere: class ViewHalfSphere:
def __init__(self, bbox, min_z_dist): def __init__(self, bbox, min_z_dist):
self.center = bbox.center self.center = bbox.center