diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 7c94162..8386a8e 100644 --- a/cfg/active_grasp.rviz +++ b/cfg/active_grasp.rviz @@ -6,7 +6,8 @@ Panels: Expanded: - /TF1/Frames1 - /Markers1/Namespaces1 - Splitter Ratio: 0.6881287693977356 + - /Marker1 + Splitter Ratio: 0.6852940917015076 Tree Height: 226 - Class: rviz/Selection Name: Selection @@ -362,6 +363,42 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /debug_pcd + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /grasp_markers + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -390,7 +427,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 0.7134475111961365 + Distance: 0.8019989132881165 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -398,17 +435,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.3979946970939636 - Y: -0.1718180924654007 - Z: 0.29551374912261963 + X: 0.5695413947105408 + Y: -0.03970015048980713 + Z: 0.45675671100616455 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4653984010219574 + Pitch: 0.295397013425827 Target Frame: - Yaw: 1.0903979539871216 + Yaw: 5.118584632873535 Saved: - Class: rviz/Orbit Distance: 1.2000000476837158 @@ -448,5 +485,5 @@ Window Geometry: Views: collapsed: true Width: 1095 - X: 1270 - Y: 138 + X: 1260 + Y: 123 diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml index 471a5d1..dfe9b55 100644 --- a/cfg/active_grasp.yaml +++ b/cfg/active_grasp.yaml @@ -2,7 +2,7 @@ bt_sim: gui: True gripper_force: 10 # scene: random - scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml + scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml hw: roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt diff --git a/scripts/run.py b/scripts/run.py index d072912..2e6fbfd 100755 --- a/scripts/run.py +++ b/scripts/run.py @@ -46,7 +46,7 @@ def main(): def create_parser(): parser = argparse.ArgumentParser() parser.add_argument("policy", type=str, choices=registry.keys()) - parser.add_argument("--runs", type=int, default=10) + parser.add_argument("--runs", type=int, default=1) parser.add_argument("--wait-for-input", action="store_true") parser.add_argument("--logdir", type=Path, default="logs") parser.add_argument("--seed", type=int, default=1) diff --git a/src/active_grasp/active_perception_policy.py b/src/active_grasp/active_perception_policy.py index c13a3ec..f95f875 100644 --- a/src/active_grasp/active_perception_policy.py +++ b/src/active_grasp/active_perception_policy.py @@ -12,6 +12,15 @@ import requests import matplotlib.pyplot as plt from vgn.grasp import ParallelJawGrasp import time +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Pose +import tf + +import sensor_msgs.point_cloud2 as pc2 +from sensor_msgs.msg import PointCloud2, PointField +import std_msgs.msg +import ros_numpy + class RealTime3DVisualizer: @@ -192,6 +201,10 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): self.updated = False self._base_url = flask_base_url + # For debugging + self.pcd_publisher = rospy.Publisher('/debug_pcd', PointCloud2, queue_size=10) + self.grasp_publisher = rospy.Publisher("/grasp_markers", MarkerArray, queue_size=10) + def request_grasping_pose(self, data): response = requests.post(f"{self._base_url}/get_gsnet_grasp", json=data) @@ -247,18 +260,29 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): 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) + target_points_radius = np.max(np.linalg.norm(target_points_list - central_point_of_target, axis=1)) scene_points_list = np.asarray(self.scene_points.cpu().numpy())[0].tolist() merged_points_list = target_points_list + scene_points_list - # gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), central_point_of_target) - gsnet_input_points = target_points_list + gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), + central_point_of_target, + radius=target_points_radius) + # gsnet_input_points = target_points_list + # gsnet_input_points = merged_points_list + self.publish_pointcloud(gsnet_input_points) gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points)) - + + # DEBUG: publish grasps + # self.publish_grasps(gsnet_grasping_poses) + # 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 - + gg['T'] = current_cam_pose.cpu().numpy().dot(np.asarray(gg['T'])) + # Now here is a mysterous bug, the grasping poses have to be rotated + # 90 degrees around y-axis to be in the correct reference frame + R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) + gg['T'][:3, :3] = gg['T'][:3, :3].dot(R) + # Convert grasping poses to ParallelJawGrasp objects grasps = [] qualities = [] @@ -271,7 +295,6 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): # Visualize grasps self.vis.grasps(self.base_frame, grasps, qualities) - time.sleep(1000000) # Filter grasps filtered_grasps = [] @@ -280,8 +303,8 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): pose = grasp.pose # tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation tip = pose.translation - # if self.bbox.is_inside(tip): - if(True): + 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) @@ -294,6 +317,59 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy): self.vis.clear_grasp() self.done = True + def publish_grasps(self, gg): + marker_array = MarkerArray() + marker_array.markers = [] + for idx, g in enumerate(gg): + g['T'] = np.asarray(g['T']) + marker = Marker() + marker.header.frame_id = "camera_depth_optical_frame" + marker.header.stamp = rospy.Time.now() + marker.ns = "grasps" + marker.id = idx + marker.type = Marker.ARROW + marker.action = Marker.ADD + marker.pose.position.x = g['T'][0, 3] + marker.pose.position.y = g['T'][1, 3] + marker.pose.position.z = g['T'][2, 3] + q = tf.transformations.quaternion_from_matrix(g['T']) + marker.pose.orientation.x = q[0] + marker.pose.orientation.y = q[1] + marker.pose.orientation.z = q[2] + marker.pose.orientation.w = q[3] + marker.scale.x = 0.1 + marker.scale.y = 0.01 + marker.scale.z = 0.01 + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker_array.markers.append(marker) + self.grasp_publisher.publish(marker_array) + + def publish_pointcloud(self, point_cloud): + point_cloud = np.asarray(point_cloud) + cloud_msg = self.create_pointcloud_msg(point_cloud) + self.pcd_publisher.publish(cloud_msg) + + def create_pointcloud_msg(self, point_cloud): + # Define the header + header = std_msgs.msg.Header() + header.stamp = rospy.Time.now() + header.frame_id = 'camera_depth_optical_frame' # Change this to your desired frame of reference + + # Define the fields for the PointCloud2 message + fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + ] + + # Create the PointCloud2 message + cloud_msg = pc2.create_cloud(header, fields, point_cloud) + + return cloud_msg + 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() diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index 3e93d23..81e74b3 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -113,9 +113,6 @@ 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 @@ -129,6 +126,9 @@ class GraspController: if(linear_d < self.move_to_target_threshold): # Arrived moving_to_The_target = False + # sleep 3s to wait for the arrival of the robot + secs = 3 + for i in range(self.control_rate*secs): r.sleep() elif(self.policy.policy_type=="multi_view"): while not self.policy.done: