update README
This commit is contained in:
0nhc
55
README.md
55
README.md
@@ -1,8 +1,11 @@
|
||||
# Roadmap
|
||||
* GS-Net filter aligned with VGN
|
||||
|
||||
# Updated installation steps fo my PC environment
|
||||
## Prerequisites
|
||||
* `Ubuntu 20.04`
|
||||
* `CUDA 12.1`
|
||||
* `ROS Noetic`. Recommended installation method: [小鱼一键安装](https://fishros.org.cn/forum/topic/20/%E5%B0%8F%E9%B1%BC%E7%9A%84%E4%B8%80%E9%94%AE%E5%AE%89%E8%A3%85%E7%B3%BB%E5%88%97/1?lang=en-US)
|
||||
* `GS-Net`: My [GS-net repo](https://github.com/0nhc/GS-Net). Run: `python flask_server.py`
|
||||
|
||||
## Installation
|
||||
```sh
|
||||
# Install Active Grasp
|
||||
sudo apt install liborocos-kdl-dev
|
||||
@@ -24,23 +27,45 @@ rosdep install --from-paths src --ignore-src -r -y
|
||||
catkin build
|
||||
|
||||
# Install Active Perception
|
||||
cd <path-to-your-ws>/src/active_grasp/src/active_grasp/active_perception/modules/module_lib/pointnet2_utils/pointnet2
|
||||
cd ws/src/active_grasp/src/active_grasp/active_perception/modules/module_lib/pointnet2_utils/pointnet2
|
||||
pip install -e .
|
||||
```
|
||||
|
||||
# Updated Features
|
||||
* Added our baseline: src/active_grasp/active_perception_policy.py
|
||||
* Added RGB and Segmentation image publishers. The segmentation ID 1 corresponds to the grasping target object.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## Quick Start
|
||||
```sh
|
||||
# Terminal 1
|
||||
conda activate active_grasp
|
||||
cd ws
|
||||
source devel/setup.bash
|
||||
roslaunch active_grasp env.launch sim:=true
|
||||
|
||||
# Terminal 2
|
||||
conda activate active_grasp
|
||||
cd ws
|
||||
source devel/setup.bash
|
||||
cd src/active_grasp
|
||||
python3 scripts/run.py ap-single-view
|
||||
```
|
||||
|
||||
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
</br>
|
||||
|
||||
# Closed-Loop Next-Best-View Planning for Target-Driven Grasping
|
||||
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
bt_sim:
|
||||
gui: True
|
||||
gripper_force: 100
|
||||
# scene: random
|
||||
scene: random
|
||||
# scene: manual
|
||||
scene: $(find active_grasp)/cfg/sim/challenging_scene_4.yaml
|
||||
# scene: $(find active_grasp)/cfg/sim/challenging_scene_4.yaml
|
||||
|
||||
hw:
|
||||
roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
|
||||
|
||||
30000
gsnet_input_points.txt
30000
gsnet_input_points.txt
File diff suppressed because it is too large
Load Diff
@@ -63,136 +63,6 @@ class RealTime3DVisualizer:
|
||||
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.max_inference_num = rospy.get_param("ap_grasp/max_inference_num")
|
||||
# self.ap_inference_engine = APInferenceEngine(self.ap_config_path)
|
||||
# self.pcdvis = RealTime3DVisualizer()
|
||||
|
||||
|
||||
# def update(self, img, seg, target_id, x, q):
|
||||
# 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)
|
||||
|
||||
# # When policy hasn't produced an available grasp
|
||||
# c = 0
|
||||
# while(c < self.max_inference_num):
|
||||
# # Inference with our model
|
||||
# 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)
|
||||
# c += 1
|
||||
# delta_rot_6d = ap_output['estimated_delta_rot_6d']
|
||||
|
||||
# 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)
|
||||
# x_d = nbv_transform
|
||||
|
||||
# # Check if this pose available
|
||||
# if(self.solve_cam_ik(self.q0, x_d)):
|
||||
# self.x_d = x_d
|
||||
# self.updated = True
|
||||
# print("Found an NBV!")
|
||||
# break
|
||||
|
||||
|
||||
# def vis_cam_pose(self, x):
|
||||
# # Integrate
|
||||
# self.views.append(x)
|
||||
# self.vis.path(self.base_frame, self.intrinsic, self.views)
|
||||
|
||||
# def vis_scene_cloud(self, img, x):
|
||||
# self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task)
|
||||
# scene_cloud = self.tsdf.get_scene_cloud()
|
||||
# self.vis.scene_cloud(self.task_frame, np.asarray(scene_cloud.points))
|
||||
|
||||
# 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 = []
|
||||
|
||||
# K = self.intrinsic.K
|
||||
# depth_shape = depth_img.shape
|
||||
# seg_shape = seg_img.shape
|
||||
# if(depth_shape == seg_shape):
|
||||
# img_shape = depth_shape
|
||||
# else:
|
||||
# print("Depth image shape and segmentation image shape are not the same")
|
||||
# return None
|
||||
|
||||
# # Convert depth image to 3D points
|
||||
# u_indices , v_indices = np.meshgrid(np.arange(img_shape[1]), np.arange(img_shape[0]))
|
||||
# x_factors = (u_indices - K[0, 2]) / K[0, 0]
|
||||
# y_factors = (v_indices - K[1, 2]) / K[1, 1]
|
||||
# z_mat = depth_img
|
||||
# x_mat = x_factors * z_mat
|
||||
# y_mat = y_factors * z_mat
|
||||
# for i in range(img_shape[0]):
|
||||
# for j in range(img_shape[1]):
|
||||
# seg_id = seg_img[i, j]
|
||||
# x = x_mat[i][j]
|
||||
# y = y_mat[i][j]
|
||||
# z = z_mat[i][j]
|
||||
# if(int(seg_id) == int(target_id)):
|
||||
# # This pixel belongs to the target object to be grasped
|
||||
# target_points.append([x,y,z])
|
||||
# else:
|
||||
# # This pixel belongs to the scene
|
||||
# scene_points.append([x,y,z])
|
||||
|
||||
# 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
|
||||
|
||||
|
||||
# def best_grasp_prediction_is_stable(self):
|
||||
# if self.best_grasp:
|
||||
# t = (self.T_task_base * self.best_grasp.pose).translation
|
||||
# i, j, k = (t / self.tsdf.voxel_size).astype(int)
|
||||
# qs = self.qual_hist[:, i, j, k]
|
||||
# if np.count_nonzero(qs) == self.T and np.mean(qs) > 0.9:
|
||||
# return True
|
||||
# return False
|
||||
|
||||
|
||||
class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
|
||||
def __init__(self, flask_base_url="http://127.0.0.1:5000"):
|
||||
super().__init__()
|
||||
|
||||
128
src/active_grasp/legacy.py
Normal file
128
src/active_grasp/legacy.py
Normal file
@@ -0,0 +1,128 @@
|
||||
# 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.max_inference_num = rospy.get_param("ap_grasp/max_inference_num")
|
||||
# self.ap_inference_engine = APInferenceEngine(self.ap_config_path)
|
||||
# self.pcdvis = RealTime3DVisualizer()
|
||||
|
||||
|
||||
# def update(self, img, seg, target_id, x, q):
|
||||
# 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)
|
||||
|
||||
# # When policy hasn't produced an available grasp
|
||||
# c = 0
|
||||
# while(c < self.max_inference_num):
|
||||
# # Inference with our model
|
||||
# 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)
|
||||
# c += 1
|
||||
# delta_rot_6d = ap_output['estimated_delta_rot_6d']
|
||||
|
||||
# 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)
|
||||
# x_d = nbv_transform
|
||||
|
||||
# # Check if this pose available
|
||||
# if(self.solve_cam_ik(self.q0, x_d)):
|
||||
# self.x_d = x_d
|
||||
# self.updated = True
|
||||
# print("Found an NBV!")
|
||||
# break
|
||||
|
||||
|
||||
# def vis_cam_pose(self, x):
|
||||
# # Integrate
|
||||
# self.views.append(x)
|
||||
# self.vis.path(self.base_frame, self.intrinsic, self.views)
|
||||
|
||||
# def vis_scene_cloud(self, img, x):
|
||||
# self.tsdf.integrate(img, self.intrinsic, x.inv() * self.T_base_task)
|
||||
# scene_cloud = self.tsdf.get_scene_cloud()
|
||||
# self.vis.scene_cloud(self.task_frame, np.asarray(scene_cloud.points))
|
||||
|
||||
# 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 = []
|
||||
|
||||
# K = self.intrinsic.K
|
||||
# depth_shape = depth_img.shape
|
||||
# seg_shape = seg_img.shape
|
||||
# if(depth_shape == seg_shape):
|
||||
# img_shape = depth_shape
|
||||
# else:
|
||||
# print("Depth image shape and segmentation image shape are not the same")
|
||||
# return None
|
||||
|
||||
# # Convert depth image to 3D points
|
||||
# u_indices , v_indices = np.meshgrid(np.arange(img_shape[1]), np.arange(img_shape[0]))
|
||||
# x_factors = (u_indices - K[0, 2]) / K[0, 0]
|
||||
# y_factors = (v_indices - K[1, 2]) / K[1, 1]
|
||||
# z_mat = depth_img
|
||||
# x_mat = x_factors * z_mat
|
||||
# y_mat = y_factors * z_mat
|
||||
# for i in range(img_shape[0]):
|
||||
# for j in range(img_shape[1]):
|
||||
# seg_id = seg_img[i, j]
|
||||
# x = x_mat[i][j]
|
||||
# y = y_mat[i][j]
|
||||
# z = z_mat[i][j]
|
||||
# if(int(seg_id) == int(target_id)):
|
||||
# # This pixel belongs to the target object to be grasped
|
||||
# target_points.append([x,y,z])
|
||||
# else:
|
||||
# # This pixel belongs to the scene
|
||||
# scene_points.append([x,y,z])
|
||||
|
||||
# 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
|
||||
|
||||
|
||||
# def best_grasp_prediction_is_stable(self):
|
||||
# if self.best_grasp:
|
||||
# t = (self.T_task_base * self.best_grasp.pose).translation
|
||||
# i, j, k = (t / self.tsdf.voxel_size).astype(int)
|
||||
# qs = self.qual_hist[:, i, j, k]
|
||||
# if np.count_nonzero(qs) == self.T and np.mean(qs) > 0.9:
|
||||
# return True
|
||||
# return False
|
||||
Reference in New Issue
Block a user