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

sim on omni_test

Browse Source
This commit is contained in:
hofee
2024-12-04 16:07:44 +08:00
parent 03bed63e8d
commit 384d8a0ca6
214 changed files with 8007 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/active_grasp/controller.py
View File

@@ -124,9 +124,7 @@ class GraspController:
depth_img, seg_image, pose, q = self.get_state()
current_p = pose.as_matrix()[:3,3]
target_p = self.policy.x_d.as_matrix()[:3,3]
linear_d = np.sqrt((current_p[0]-target_p[0])**2+
(current_p[1]-target_p[1])**2+
(current_p[2]-target_p[2])**2)
linear_d = np.linalg.norm(current_p - target_p)
if(linear_d < self.move_to_target_threshold):
# Arrived
moving_to_The_target = False
@@ -139,7 +137,9 @@ class GraspController:
depth_img, seg_image, pose, q = self.get_state()
target_seg_id = self.get_target_id(TargetIDRequest()).id
support_seg_id = self.get_support_id(TargetIDRequest()).id
self.policy.update(depth_img, seg_image, target_seg_id, support_seg_id, pose, q)
# print(target_seg_id, support_seg_id)
# import ipdb; ipdb.set_trace()
self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
r.sleep()
else:
print("Unsupported policy type: "+str(self.policy.policy_type))