diff --git a/active_grasp/policy.py b/active_grasp/policy.py index f621eb6..16f1222 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -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() diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index 8537d5c..d75f3b8 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -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) diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py index 3e7a7b0..9c7f773 100644 --- a/active_grasp/visualization.py +++ b/active_grasp/visualization.py @@ -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, diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index c92437d..993de53 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -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