From 9b8dcac20286d6215bb52e699a9aaa02fd190803 Mon Sep 17 00:00:00 2001 From: 0nhc Date: Sun, 13 Oct 2024 03:52:33 -0500 Subject: [PATCH] multi-view policy test passed --- cfg/active_grasp.rviz | 35 +++--- requirements.txt | 3 +- src/active_grasp/__init__.py | 4 +- src/active_grasp/active_perception_demo.py | 6 - src/active_grasp/active_perception_policy.py | 113 +++++++++++++++---- 5 files changed, 112 insertions(+), 49 deletions(-) diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz index 02be1e3..c41ae6c 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: 710 + Tree Height: 371 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -25,7 +25,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: SceneCloud + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -290,8 +290,7 @@ Visualization Manager: Marker Topic: visualization_marker_array Name: Markers Namespaces: - bbox: true - roi: true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -410,7 +409,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.3440001010894775 + Distance: 2.024759531021118 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -418,17 +417,17 @@ Visualization Manager: Value: false Field of View: 0.7799999713897705 Focal Point: - X: 0.40848347544670105 - Y: 0.02209819294512272 - Z: 0.4073639214038849 + X: 0.36431100964546204 + Y: -0.015723222866654396 + Z: 0.38234013319015503 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.25999999046325684 + Pitch: 0.4999997615814209 Target Frame: - Yaw: 1.0399999618530273 + Yaw: 5.183177471160889 Saved: - Class: rviz/Orbit Distance: 1.2000000476837158 @@ -452,17 +451,17 @@ Visualization Manager: Yaw: 1.0399999618530273 Window Geometry: ColorImage: - collapsed: false + collapsed: true DepthImage: collapsed: false Displays: - collapsed: false - Height: 960 - Hide Left Dock: false + collapsed: true + Height: 787 + Hide Left Dock: true Hide Right Dock: true InfraImage: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -471,6 +470,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1157 - X: 1309 - Y: 90 + Width: 1023 + X: 1275 + Y: 61 diff --git a/requirements.txt b/requirements.txt index 8da1092..b229d87 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,4 +7,5 @@ rospkg scikit-image opencv-python numpy==1.22.0 -pyassimp \ No newline at end of file +pyassimp +open3d==0.18.0 \ No newline at end of file diff --git a/src/active_grasp/__init__.py b/src/active_grasp/__init__.py index 31f81e5..6372414 100644 --- a/src/active_grasp/__init__.py +++ b/src/active_grasp/__init__.py @@ -1,11 +1,11 @@ from .policy import register from .baselines import * from .nbv import NextBestView -from.active_perception_policy import ActivePerceptionPolicy +from.active_perception_policy import ActivePerceptionMultiViewPolicy register("initial-view", InitialView) register("top-view", TopView) register("top-trajectory", TopTrajectory) register("fixed-trajectory", FixedTrajectory) register("nbv", NextBestView) -register("ap", ActivePerceptionPolicy) \ No newline at end of file +register("ap-multi-view", ActivePerceptionMultiViewPolicy) \ No newline at end of file diff --git a/src/active_grasp/active_perception_demo.py b/src/active_grasp/active_perception_demo.py index fd2b95c..cbdda2f 100644 --- a/src/active_grasp/active_perception_demo.py +++ b/src/active_grasp/active_perception_demo.py @@ -71,12 +71,6 @@ if __name__ == "__main__": ''' Initialize Test Data ''' test_scene = torch.rand(1, 1024, 3).to("cuda:0") test_target = torch.rand(1, 1024, 3).to("cuda:0") - test_delta_rot_6d = torch.rand(1, 6).to("cuda:0") - a = test_delta_rot_6d[:, :3] - b = test_delta_rot_6d[:, 3:] - a_norm = a / a.norm(dim=1, keepdim=True) - b_norm = b / b.norm(dim=1, keepdim=True) - normalized_test_delta_rot_6d = torch.cat((a_norm, b_norm), dim=1) test_data = { 'target_pts': test_target, 'scene_pts': test_scene, diff --git a/src/active_grasp/active_perception_policy.py b/src/active_grasp/active_perception_policy.py index 6cf7d44..e19590a 100644 --- a/src/active_grasp/active_perception_policy.py +++ b/src/active_grasp/active_perception_policy.py @@ -4,43 +4,109 @@ import numpy as np import rospy from .policy import MultiViewPolicy from .timer import Timer - from .active_perception_demo import APInferenceEngine +from robot_helpers.spatial import Transform +import torch +import torch.nn.functional as F -class ActivePerceptionPolicy(MultiViewPolicy): +import matplotlib.pyplot as plt + + +class RealTime3DVisualizer: + def __init__(self): + points = np.random.rand(1, 1, 3) + self.points = points[0] # Extract the points (n, 3) + self.fig = plt.figure() + self.ax = self.fig.add_subplot(111, projection='3d') + + # Initial plot setup + self.scatter = self.ax.scatter(self.points[:, 0], self.points[:, 1], self.points[:, 2], c='b', marker='o') + + # Set labels for each axis + self.ax.set_xlabel('X') + self.ax.set_ylabel('Y') + self.ax.set_zlabel('Z') + + # Set title + self.ax.set_title('Real-time 3D Points Visualization') + + # Show the plot in interactive mode + plt.ion() + plt.show() + + + def update_points(self, new_points): + # Ensure the points have the expected shape (1, n, 3) + assert new_points.shape[0] == 1 and new_points.shape[2] == 3, "Input points must have shape (1, n, 3)" + + # Update the stored points + self.points = new_points[0] # Extract the points (n, 3) + + # Remove the old scatter plot and draw new points + self.scatter.remove() + self.scatter = self.ax.scatter(self.points[:, 0], self.points[:, 1], self.points[:, 2], c='b', marker='o') + + # Pause briefly to allow the plot to update + plt.pause(0.001) + + +class ActivePerceptionMultiViewPolicy(MultiViewPolicy): def __init__(self): super().__init__() self.max_views = rospy.get_param("ap_grasp/max_views") self.ap_config_path = rospy.get_param("ap_grasp/ap_config_path") self.ap_inference_engine = APInferenceEngine(self.ap_config_path) + self.pcdvis = RealTime3DVisualizer() + def activate(self, bbox, view_sphere): super().activate(bbox, view_sphere) def update(self, img, seg, target_id, x, q): - target_points, scene_points = self.depth_image_to_ap_input(img, seg, target_id) - # if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable(): - # self.done = True - # else: - # with Timer("state_update"): - # self.integrate(img, x, q) - # with Timer("view_generation"): - # views = self.generate_views(q) - # with Timer("ig_computation"): - # gains = [self.ig_fn(v, self.downsample) for v in views] - # with Timer("cost_computation"): - # costs = [self.cost_fn(v) for v in views] - # utilities = gains / np.sum(gains) - costs / np.sum(costs) - # self.vis.ig_views(self.base_frame, self.intrinsic, views, utilities) - # i = np.argmax(utilities) - # nbv, gain = views[i], gains[i] + if len(self.views) > self.max_views or self.best_grasp_prediction_is_stable(): + self.done = True + else: + with Timer("state_update"): + self.integrate(img, x, q) + with Timer("view_generation"): + target_points, scene_points = self.depth_image_to_ap_input(img, seg, target_id) + ap_input = {'target_pts': target_points, + 'scene_pts': scene_points} + ap_output = self.ap_inference_engine.inference(ap_input) + delta_rot_6d = ap_output['estimated_delta_rot_6d'] - # if gain < self.min_gain and len(self.views) > self.T: - # self.done = True - - # self.x_d = nbv + current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0") + est_delta_rot_mat = self.rotation_6d_to_matrix_tensor_batch(delta_rot_6d)[0] + look_at_center = torch.from_numpy(self.bbox.center).float().to("cuda:0") + nbv_tensor = self.get_transformed_mat(current_cam_pose, + est_delta_rot_mat, + look_at_center) + nbv_numpy = nbv_tensor.cpu().numpy() + nbv_transform = Transform.from_matrix(nbv_numpy) + self.x_d = nbv_transform + + def rotation_6d_to_matrix_tensor_batch(self, d6: torch.Tensor) -> torch.Tensor: + a1, a2 = d6[..., :3], d6[..., 3:] + b1 = F.normalize(a1, dim=-1) + b2 = a2 - (b1 * a2).sum(-1, keepdim=True) * b1 + b2 = F.normalize(b2, dim=-1) + b3 = torch.cross(b1, b2, dim=-1) + return torch.stack((b1, b2, b3), dim=-2) + + + def get_transformed_mat(self, src_mat, delta_rot, target_center_w): + src_rot = src_mat[:3, :3] + dst_rot = src_rot @ delta_rot.T + dst_mat = torch.eye(4).to(dst_rot.device) + dst_mat[:3, :3] = dst_rot + distance = torch.norm(target_center_w - src_mat[:3, 3]) + z_axis_camera = dst_rot[:3, 2].reshape(-1) + new_camera_position_w = target_center_w - distance * z_axis_camera + dst_mat[:3, 3] = new_camera_position_w + return dst_mat + def depth_image_to_ap_input(self, depth_img, seg_img, target_id): target_points = [] scene_points = [] @@ -76,8 +142,11 @@ class ActivePerceptionPolicy(MultiViewPolicy): target_points = np.asarray(target_points) target_points = target_points.reshape(1, target_points.shape[0], 3) + self.pcdvis.update_points(target_points) + target_points = torch.from_numpy(target_points).float().to("cuda:0") scene_points = np.asarray(scene_points) scene_points = scene_points.reshape(1, scene_points.shape[0], 3) + scene_points = torch.from_numpy(scene_points).float().to("cuda:0") return target_points, scene_points