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"):
tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size
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
self.qual_hist[t, ...] = out.qual
with Timer("grasp_selection"):
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()

2
active_grasp/simulation.py
View File

@ -88,7 +88,7 @@ class Scene:
self.object_uids = []
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):
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))
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 = []
for i, grasp in enumerate(grasps):
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)
self.grasp_marker_count = len(markers)
self.draw(markers)
@ -99,8 +98,8 @@ class Visualizer:
marker = create_line_list_marker(
frame,
Transform.identity(),
[0.002, 0.0, 0.0],
[0.6, 0.6, 0.6],
[0.001, 0.0, 0.0],
[0.9, 0.9, 0.9],
lines,
"rays",
)
@ -124,16 +123,19 @@ class Visualizer:
"path",
0,
)
lines = create_line_strip_marker(
frame,
Transform.identity(),
[0.005, 0.0, 0.0],
color,
points,
"path",
1,
)
self.draw([spheres, lines])
markers = [spheres]
if len(poses) > 1:
lines = create_line_strip_marker(
frame,
Transform.identity(),
[0.005, 0.0, 0.0],
color,
points,
"path",
1,
)
markers.append(lines)
self.draw(markers)
def point(self, frame, point):
marker = create_sphere_marker(
@ -151,7 +153,6 @@ class Visualizer:
def quality(self, frame, voxel_size, quality, threshold=0.9):
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)
self.quality_pub.publish(msg)
@ -159,14 +160,13 @@ class Visualizer:
msg = to_cloud_msg(frame, np.asarray(cloud.points))
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)
scale = [0.002, 0.0, 0.0]
near, far = 0.0, 0.02
markers = []
for i, (view, value) in enumerate(zip(views, values)):
color = cmap((value - vmin) / (vmax - vmin))
color = [color[0], color[1], color[2], alpha]
marker = create_cam_view_marker(
frame,
view,

6
cfg/active_grasp.yaml
View File

@ -1,11 +1,11 @@
bt_sim:
gui: True
cam_noise: False
gui: False
cam_noise: True
scene: test1.yaml
grasp_controller:
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
linear_vel: 0.05
policy_rate: 5