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

goodnight

Browse Source
This commit is contained in:
0nhc 2024-10-20 17:20:09 -05:00
parent 37591fcaa8
commit c8f0354550
3 changed files with 40 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cfg/active_grasp.rviz
View File

@ -7,7 +7,7 @@ Panels:
- /TF1/Frames1 - /TF1/Frames1
- /Markers1/Namespaces1 - /Markers1/Namespaces1
Splitter Ratio: 0.6881287693977356 Splitter Ratio: 0.6881287693977356
Tree Height: 181 Tree Height: 226
- Class: rviz/Selection - Class: rviz/Selection
Name: Selection Name: Selection
- Class: rviz/Tool Properties - Class: rviz/Tool Properties
@ -255,6 +255,8 @@ Visualization Manager:
Name: Markers Name: Markers
Namespaces: Namespaces:
bbox: true bbox: true
grasp: true
grasps: true
path: true path: true
roi: true roi: true
views: true views: true
@ -388,7 +390,7 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/Orbit Class: rviz/Orbit
Distance: 1.8333022594451904 Distance: 0.7134475111961365
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
@ -396,17 +398,17 @@ Visualization Manager:
Value: false Value: false
Field of View: 0.7853981852531433 Field of View: 0.7853981852531433
Focal Point: Focal Point:
X: 0.18340304493904114 X: 0.3979946970939636
Y: -0.2598724365234375 Y: -0.1718180924654007
Z: 0.2933075726032257 Z: 0.29551374912261963
Focal Shape Fixed Size: false Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806 Focal Shape Size: 0.05000000074505806
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 0.4453985095024109 Pitch: 0.4653984010219574
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Yaw: 0.6903981566429138 Yaw: 1.0903979539871216
Saved: Saved:
- Class: rviz/Orbit - Class: rviz/Orbit
Distance: 1.2000000476837158 Distance: 1.2000000476837158
@ -436,7 +438,7 @@ Window Geometry:
Hide Right Dock: true Hide Right Dock: true
Image: Image:
collapsed: false collapsed: false
QMainWindow State: 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 QMainWindow State: 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
Selection: Selection:
collapsed: false collapsed: false
Time: Time:
@ -446,5 +448,5 @@ Window Geometry:
Views: Views:
collapsed: true collapsed: true
Width: 1095 Width: 1095
X: 1232 X: 1270
Y: 51 Y: 138

34
src/active_grasp/active_perception_policy.py
View File

@ -11,6 +11,7 @@ import torch.nn.functional as F
import requests import requests
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from vgn.grasp import ParallelJawGrasp from vgn.grasp import ParallelJawGrasp
import time
class RealTime3DVisualizer: class RealTime3DVisualizer:
@ -239,37 +240,48 @@ 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 and moved to that camera pose # Policy has produced an available nbv and moved to that camera pose
if(self.updated == True): 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) 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() 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() scene_points_list = np.asarray(self.scene_points.cpu().numpy())[0].tolist()
merged_points_list = target_points_list + scene_points_list merged_points_list = target_points_list + scene_points_list
gsnet_grasping_poses = np.asarray(self.request_grasping_pose(merged_points_list)) # gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), central_point_of_target)
current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0") 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 # 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: for gg in gsnet_grasping_poses:
T = np.asarray(gg['T']) T = np.asarray(gg['T'])
gg['T'] = current_cam_pose.cpu().numpy() @ T gg['T'] = current_cam_pose.cpu().numpy() @ T
# Convert grasping poses to grasp objects
# Convert grasping poses to ParallelJawGrasp objects
grasps = [] grasps = []
qualities = [] qualities = []
for gg in gsnet_grasping_poses: for gg in gsnet_grasping_poses:
T = Transform.from_matrix(np.asarray(gg['T'])) T = Transform.from_matrix(np.asarray(gg['T']))
width = 0.1 width = 0.075
grasp = ParallelJawGrasp(T, width) grasp = ParallelJawGrasp(T, width)
grasps.append(grasp) grasps.append(grasp)
qualities.append(gg['score']) qualities.append(gg['score'])
# Visualize grasps
self.vis.grasps(self.base_frame, grasps, qualities)
time.sleep(1000000)
# Filter grasps # Filter grasps
filtered_grasps = [] filtered_grasps = []
filtered_qualities = [] filtered_qualities = []
for grasp, quality in zip(grasps, qualities): for grasp, quality in zip(grasps, qualities):
pose = grasp.pose pose = grasp.pose
tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation # tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
if self.bbox.is_inside(tip): tip = pose.translation
grasp.pose = pose # if self.bbox.is_inside(tip):
if(True):
q_grasp = self.solve_ee_ik(q, pose * self.T_grasp_ee) q_grasp = self.solve_ee_ik(q, pose * self.T_grasp_ee)
if q_grasp is not None: if q_grasp is not None:
filtered_grasps.append(grasp) filtered_grasps.append(grasp)
@ -281,7 +293,11 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
self.best_grasp = None self.best_grasp = None
self.vis.clear_grasp() self.vis.clear_grasp()
self.done = True 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): def deactivate(self):
self.vis.clear_ig_views() self.vis.clear_ig_views()
self.updated = False self.updated = False

6
src/active_grasp/controller.py
View File

@ -113,6 +113,9 @@ class GraspController:
while not self.policy.done: while not self.policy.done:
depth_img, seg_image, pose, q = self.get_state() depth_img, seg_image, pose, q = self.get_state()
target_seg_id = self.get_target_id(TargetIDRequest()).id 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) self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
# Wait for the robot to move to its desired camera pose # Wait for the robot to move to its desired camera pose
moving_to_The_target = True moving_to_The_target = True
@ -127,9 +130,6 @@ class GraspController:
# Arrived # Arrived
moving_to_The_target = False moving_to_The_target = False
r.sleep() r.sleep()
# sleep 3s
for i in range(self.control_rate*3):
r.sleep()
elif(self.policy.policy_type=="multi_view"): elif(self.policy.policy_type=="multi_view"):
while not self.policy.done: while not self.policy.done:
depth_img, seg_image, pose, q = self.get_state() depth_img, seg_image, pose, q = self.get_state()