hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

interfaced with latest gsnet flask server

Browse Source
This commit is contained in:
0nhc 2024-10-20 16:07:39 -05:00
parent ca9b6733f0
commit 37591fcaa8
2 changed files with 43 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -1,5 +1,5 @@
# Roadmap # Roadmap
* Interface with GS-Net * GS-Net filter aligned with VGN
# Updated installation steps fo my PC environment # Updated installation steps fo my PC environment

67
src/active_grasp/active_perception_policy.py
View File

@ -207,9 +207,9 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
# When policy hasn't produced an available grasp # When policy hasn't produced an available grasp
while(self.updated == False): while(self.updated == False):
# Inference with our model # Inference with our model
self.target_points, scene_points = self.depth_image_to_ap_input(img, seg, target_id) self.target_points, self.scene_points = self.depth_image_to_ap_input(img, seg, target_id)
ap_input = {'target_pts': self.target_points, ap_input = {'target_pts': self.target_points,
'scene_pts': scene_points} 'scene_pts': self.scene_points}
ap_output = self.ap_inference_engine.inference(ap_input) ap_output = self.ap_inference_engine.inference(ap_input)
delta_rot_6d = ap_output['estimated_delta_rot_6d'] delta_rot_6d = ap_output['estimated_delta_rot_6d']
@ -224,8 +224,7 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
look_at_center_T[:3, 3] = look_at_center.cpu().numpy() look_at_center_T[:3, 3] = look_at_center.cpu().numpy()
look_at_center_T = current_cam_pose.cpu().numpy() @ look_at_center_T look_at_center_T = current_cam_pose.cpu().numpy() @ look_at_center_T
look_at_center = torch.from_numpy(look_at_center_T[:3, 3]).float().to("cuda:0") look_at_center = torch.from_numpy(look_at_center_T[:3, 3]).float().to("cuda:0")
# print(f"Central Point of Target: {central_point_of_target}, length: {np.linalg.norm(central_point_of_target)}") # Get the NBV
# print(f"camera position: {current_cam_pose[:3, 3]}")
nbv_tensor = self.get_transformed_mat(current_cam_pose, nbv_tensor = self.get_transformed_mat(current_cam_pose,
est_delta_rot_mat, est_delta_rot_mat,
look_at_center) look_at_center)
@ -240,18 +239,47 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
self.updated = True self.updated = True
print("Found an NBV!") print("Found an NBV!")
return return
# Policy has produced an available nbv # Policy has produced an available nbv and moved to that camera pose
if(self.updated == True): if(self.updated == True):
data = np.asarray(self.target_points.cpu().numpy())[0].tolist() # Visualize the new camera pose
best_grasp_T = np.asarray(self.request_grasping_pose(data)) 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()
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") current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0")
best_grasp_T = current_cam_pose.cpu().numpy() @ best_grasp_T # Convert all grasping poses' reference frame to arm frame
pose = Transform.from_matrix(best_grasp_T) 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
grasps = []
qualities = []
for gg in gsnet_grasping_poses:
T = Transform.from_matrix(np.asarray(gg['T']))
width = 0.1 width = 0.1
grasp = ParallelJawGrasp(pose, width) grasp = ParallelJawGrasp(T, width)
self.best_grasp = grasp grasps.append(grasp)
self.vis.grasp(self.base_frame, self.best_grasp, 0.9) qualities.append(gg['score'])
# self.generate_grasp(q)
# 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
q_grasp = self.solve_ee_ik(q, pose * self.T_grasp_ee)
if q_grasp is not None:
filtered_grasps.append(grasp)
filtered_qualities.append(quality)
if len(filtered_grasps) > 0:
self.best_grasp, quality = self.select_best_grasp(filtered_grasps, filtered_qualities)
self.vis.grasp(self.base_frame, self.best_grasp, quality)
else:
self.best_grasp = None
self.vis.clear_grasp()
self.done = True self.done = True
def deactivate(self): def deactivate(self):
@ -268,19 +296,6 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
scene_cloud = self.tsdf.get_scene_cloud() scene_cloud = self.tsdf.get_scene_cloud()
self.vis.scene_cloud(self.task_frame, np.asarray(scene_cloud.points)) self.vis.scene_cloud(self.task_frame, np.asarray(scene_cloud.points))
def generate_gsnet_grasp(self, q):
tsdf_grid = self.tsdf.get_grid()
out = self.vgn.predict(tsdf_grid)
self.vis.quality(self.task_frame, self.tsdf.voxel_size, out.qual, 0.9)
grasps, qualities = self.filter_grasps(out, q)
if len(grasps) > 0:
self.best_grasp, quality = self.select_best_grasp(grasps, qualities)
self.vis.grasp(self.base_frame, self.best_grasp, quality)
else:
self.best_grasp = None
self.vis.clear_grasp()
def generate_grasp(self, q): def generate_grasp(self, q):
tsdf_grid = self.tsdf.get_grid() tsdf_grid = self.tsdf.get_grid()
out = self.vgn.predict(tsdf_grid) out = self.vgn.predict(tsdf_grid)