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

ap-single-view baseline succeeded

Browse Source
This commit is contained in:
0nhc 2024-10-13 06:40:54 -05:00
parent 2a5192e9db
commit 79d709d1ac
2 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cfg/active_grasp.yaml
View File

@ -12,7 +12,7 @@ grasp_controller:
ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to panda_link8 ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to panda_link8
control_rate: 100 control_rate: 100
linear_vel: 0.05 linear_vel: 0.05
move_arm_time: 6 move_to_target_threshold: 0.05 # meter
camera: camera:
frame_id: camera_depth_optical_frame frame_id: camera_depth_optical_frame
info_topic: /camera/depth/camera_info info_topic: /camera/depth/camera_info

15
src/active_grasp/controller.py
View File

@ -36,7 +36,7 @@ class GraspController:
self.min_z_dist = rospy.get_param("~camera/min_z_dist") self.min_z_dist = rospy.get_param("~camera/min_z_dist")
self.control_rate = rospy.get_param("~control_rate") self.control_rate = rospy.get_param("~control_rate")
self.linear_vel = rospy.get_param("~linear_vel") self.linear_vel = rospy.get_param("~linear_vel")
self.move_arm_time = rospy.get_param("~move_arm_time") self.move_to_target_threshold = rospy.get_param("~move_to_target_threshold")
self.policy_rate = rospy.get_param("policy/rate") self.policy_rate = rospy.get_param("policy/rate")
def init_service_proxies(self): def init_service_proxies(self):
@ -113,8 +113,17 @@ class GraspController:
target_seg_id = self.get_target_id(TargetIDRequest()).id target_seg_id = self.get_target_id(TargetIDRequest()).id
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
ticks = self.move_arm_time*self.policy_rate moving_to_The_target = True
for i in range(ticks): while(moving_to_The_target):
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)
if(linear_d < self.move_to_target_threshold):
# Arrived
moving_to_The_target = False
r.sleep() r.sleep()
# Wait for a zero command to be sent to the robot. # Wait for a zero command to be sent to the robot.