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-09-13 18:29:41 +02:00
parent e9344a4db2
commit 8e31931609
4 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
active_grasp/policy.py
View File

@ -138,14 +138,14 @@ class MultiViewPolicy(Policy):
with Timer("grasp_prediction"): with Timer("grasp_prediction"):
tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
out = self.vgn.predict(tsdf_grid) out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, self.tsdf.voxel_size, out.qual, 0.8) self.vis.quality(self.task_frame, self.tsdf.voxel_size, out.qual, 0.9)
t = (len(self.views) - 1) % self.T t = (len(self.views) - 1) % self.T
self.qual_hist[t, ...] = out.qual self.qual_hist[t, ...] = out.qual
with Timer("grasp_selection"): with Timer("grasp_selection"):
grasps = select_grid(voxel_size, out, threshold=self.qual_threshold) grasps = select_grid(voxel_size, out, threshold=self.qual_threshold)
grasps, scores = self.sort_grasps(grasps, q) grasps, _ = self.sort_grasps(grasps, q)
self.vis.clear_grasps() self.vis.clear_grasps()

2
active_grasp/simulation.py
View File

@ -88,7 +88,7 @@ class Scene:
self.object_uids = [] self.object_uids = []
def load_support(self, pos): def load_support(self, pos):
self.support_uid = p.loadURDF(str(self.support_urdf), pos, globalScaling=0.2) self.support_uid = p.loadURDF(str(self.support_urdf), pos, globalScaling=0.3)
def remove_support(self): def remove_support(self):
p.removeBody(self.support_uid) p.removeBody(self.support_uid)

34
active_grasp/visualization.py
View File

@ -73,11 +73,10 @@ class Visualizer:
color = cmap((grasp.quality - qmin) / (qmax - qmin)) color = cmap((grasp.quality - qmin) / (qmax - qmin))
self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.006)) self.draw(create_grasp_markers(frame, grasp, color, "best_grasp", radius=0.006))
def grasps(self, frame, grasps, qmin=0.5, qmax=1.0, alpha=0.8): def grasps(self, frame, grasps, qmin=0.5, qmax=1.0):
markers = [] markers = []
for i, grasp in enumerate(grasps): for i, grasp in enumerate(grasps):
color = cmap((grasp.quality - qmin) / (qmax - qmin)) color = cmap((grasp.quality - qmin) / (qmax - qmin))
color = [color[0], color[1], color[2], alpha]
markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i) markers += create_grasp_markers(frame, grasp, color, "grasps", 4 * i)
self.grasp_marker_count = len(markers) self.grasp_marker_count = len(markers)
self.draw(markers) self.draw(markers)
@ -99,8 +98,8 @@ class Visualizer:
marker = create_line_list_marker( marker = create_line_list_marker(
frame, frame,
Transform.identity(), Transform.identity(),
[0.002, 0.0, 0.0], [0.001, 0.0, 0.0],
[0.6, 0.6, 0.6], [0.9, 0.9, 0.9],
lines, lines,
"rays", "rays",
) )
@ -124,16 +123,19 @@ class Visualizer:
"path", "path",
0, 0,
) )
lines = create_line_strip_marker( markers = [spheres]
frame, if len(poses) > 1:
Transform.identity(), lines = create_line_strip_marker(
[0.005, 0.0, 0.0], frame,
color, Transform.identity(),
points, [0.005, 0.0, 0.0],
"path", color,
1, points,
) "path",
self.draw([spheres, lines]) 1,
)
markers.append(lines)
self.draw(markers)
def point(self, frame, point): def point(self, frame, point):
marker = create_sphere_marker( marker = create_sphere_marker(
@ -151,7 +153,6 @@ class Visualizer:
def quality(self, frame, voxel_size, quality, threshold=0.9): def quality(self, frame, voxel_size, quality, threshold=0.9):
points, values = grid_to_map_cloud(voxel_size, quality, threshold) points, values = grid_to_map_cloud(voxel_size, quality, threshold)
values = (values - 0.9) / (1.0 - 0.9) # to increase contrast
msg = to_cloud_msg(frame, points, intensities=values) msg = to_cloud_msg(frame, points, intensities=values)
self.quality_pub.publish(msg) self.quality_pub.publish(msg)
@ -159,14 +160,13 @@ class Visualizer:
msg = to_cloud_msg(frame, np.asarray(cloud.points)) msg = to_cloud_msg(frame, np.asarray(cloud.points))
self.scene_cloud_pub.publish(msg) self.scene_cloud_pub.publish(msg)
def views(self, frame, intrinsic, views, values, alpha=0.8): def views(self, frame, intrinsic, views, values):
vmin, vmax = min(values), max(values) vmin, vmax = min(values), max(values)
scale = [0.002, 0.0, 0.0] scale = [0.002, 0.0, 0.0]
near, far = 0.0, 0.02 near, far = 0.0, 0.02
markers = [] markers = []
for i, (view, value) in enumerate(zip(views, values)): for i, (view, value) in enumerate(zip(views, values)):
color = cmap((value - vmin) / (vmax - vmin)) color = cmap((value - vmin) / (vmax - vmin))
color = [color[0], color[1], color[2], alpha]
marker = create_cam_view_marker( marker = create_cam_view_marker(
frame, frame,
view, view,

6
cfg/active_grasp.yaml
View File

@ -1,11 +1,11 @@
bt_sim: bt_sim:
gui: True gui: False
cam_noise: False cam_noise: True
scene: test1.yaml scene: test1.yaml
grasp_controller: grasp_controller:
base_frame_id: panda_link0 base_frame_id: panda_link0
ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to the moveit ee ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to panda_link8
control_rate: 30 control_rate: 30
linear_vel: 0.05 linear_vel: 0.05
policy_rate: 5 policy_rate: 5