diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 7399ccd..7c94162 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -7,7 +7,7 @@ Panels: - /TF1/Frames1 - /Markers1/Namespaces1 Splitter Ratio: 0.6881287693977356 - Tree Height: 181 + Tree Height: 226 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -255,6 +255,8 @@ Visualization Manager: Name: Markers Namespaces: bbox: true + grasp: true + grasps: true path: true roi: true views: true @@ -388,7 +390,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.8333022594451904 + Distance: 0.7134475111961365 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -396,17 +398,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.18340304493904114 - Y: -0.2598724365234375 - Z: 0.2933075726032257 + X: 0.3979946970939636 + Y: -0.1718180924654007 + Z: 0.29551374912261963 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4453985095024109 + Pitch: 0.4653984010219574 Target Frame: - Yaw: 0.6903981566429138 + Yaw: 1.0903979539871216 Saved: - Class: rviz/Orbit Distance: 1.2000000476837158 @@ -436,7 +438,7 @@ Window Geometry: Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -446,5 +448,5 @@ Window Geometry: Views: collapsed: true Width: 1095 - X: 1232 - Y: 51 + X: 1270 + Y: 138 diff --git a/src/active_grasp/active_perception_policy.py b/src/active_grasp/active_perception_policy.py index 44dd4ec..c13a3ec 100644 --- a/src/active_grasp/active_perception_policy.py +++ b/src/active_grasp/active_perception_policy.py @@ -11,6 +11,7 @@ import torch.nn.functional as F import requests import matplotlib.pyplot as plt from vgn.grasp import ParallelJawGrasp +import time class RealTime3DVisualizer: @@ -239,37 +240,48 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): self.updated = True print("Found an NBV!") return + # Policy has produced an available nbv and moved to that camera pose if(self.updated == True): - # Visualize the new camera pose + # Request grasping poses from GSNet self.target_points, self.scene_points = self.depth_image_to_ap_input(img, seg, target_id) target_points_list = np.asarray(self.target_points.cpu().numpy())[0].tolist() + central_point_of_target = np.mean(target_points_list, axis=0) scene_points_list = np.asarray(self.scene_points.cpu().numpy())[0].tolist() merged_points_list = target_points_list + scene_points_list - gsnet_grasping_poses = np.asarray(self.request_grasping_pose(merged_points_list)) - current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0") + # gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), central_point_of_target) + gsnet_input_points = target_points_list + gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points)) + # Convert all grasping poses' reference frame to arm frame + current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0") for gg in gsnet_grasping_poses: T = np.asarray(gg['T']) gg['T'] = current_cam_pose.cpu().numpy() @ T - # Convert grasping poses to grasp objects + + # Convert grasping poses to ParallelJawGrasp objects grasps = [] qualities = [] for gg in gsnet_grasping_poses: T = Transform.from_matrix(np.asarray(gg['T'])) - width = 0.1 + width = 0.075 grasp = ParallelJawGrasp(T, width) grasps.append(grasp) qualities.append(gg['score']) + # Visualize grasps + self.vis.grasps(self.base_frame, grasps, qualities) + time.sleep(1000000) + # Filter grasps filtered_grasps = [] filtered_qualities = [] for grasp, quality in zip(grasps, qualities): pose = grasp.pose - tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation - if self.bbox.is_inside(tip): - grasp.pose = pose + # tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation + tip = pose.translation + # if self.bbox.is_inside(tip): + if(True): q_grasp = self.solve_ee_ik(q, pose * self.T_grasp_ee) if q_grasp is not None: filtered_grasps.append(grasp) @@ -281,7 +293,11 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): self.best_grasp = None self.vis.clear_grasp() self.done = True - + + def crop_pts_sphere(self, points, crop_center, radius=0.2): + crop_mask = np.linalg.norm(points - crop_center, axis=1) < radius + return points[crop_mask].tolist() + def deactivate(self): self.vis.clear_ig_views() self.updated = False diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index 9338865..3e93d23 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -113,6 +113,9 @@ class GraspController: while not self.policy.done: depth_img, seg_image, pose, q = self.get_state() target_seg_id = self.get_target_id(TargetIDRequest()).id + # sleep 1s + for i in range(self.control_rate*1): + r.sleep() self.policy.update(depth_img, seg_image, target_seg_id, pose, q) # Wait for the robot to move to its desired camera pose moving_to_The_target = True @@ -127,9 +130,6 @@ class GraspController: # Arrived moving_to_The_target = False r.sleep() - # sleep 3s - for i in range(self.control_rate*3): - r.sleep() elif(self.policy.policy_type=="multi_view"): while not self.policy.done: depth_img, seg_image, pose, q = self.get_state()