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

Select local_maxima instead of grid

Browse Source
This commit is contained in:
Michel Breyer
2021-10-11 14:31:31 +02:00
parent fcee4a9517
commit 17b9347976
3 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/active_grasp/policy.py
View File

@@ -26,7 +26,7 @@ class Policy:
info_topic = rospy.get_param("~camera/info_topic")
msg = rospy.wait_for_message(info_topic, CameraInfo, rospy.Duration(2.0))
self.intrinsic = from_camera_info_msg(msg)
self.qual_threshold = rospy.get_param("vgn/qual_threshold")
self.qual_thresh = rospy.get_param("vgn/qual_threshold")
def init_robot_model(self):
self.model = KDLModel.from_parameter_server(self.base_frame, self.cam_frame)
@@ -91,7 +91,7 @@ class Policy:
return filtered_grasps[i], qualities[i], scores[i]
def score_fn(self, grasp, quality, q, q_grasp):
return -np.linalg.norm(q - q_grasp)
return grasp.pose.translation[2]
class SingleViewPolicy(Policy):
@@ -107,7 +107,7 @@ class SingleViewPolicy(Policy):
out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, voxel_size, out.qual, 0.5)
grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
grasps, qualities = select_local_maxima(voxel_size, out, self.qual_thresh)
grasps, _ = self.sort_grasps(grasps, qualities, q)
if len(grasps) > 0:
@@ -145,7 +145,7 @@ class MultiViewPolicy(Policy):
self.qual_hist[t, ...] = out.qual
with Timer("grasp_selection"):
grasps, qualities = select_grid(voxel_size, out, self.qual_threshold)
grasps, qualities = select_local_maxima(voxel_size, out, self.qual_thresh)
grasps, qualities, _ = self.sort_grasps(grasps, qualities, q)
if len(grasps) > 0: