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

add offset for gsnet-generated grasp poses

Browse Source
This commit is contained in:
0nhc 2024-11-04 00:53:23 -06:00
parent 9304832801
commit 4099a0574d
5 changed files with 15008 additions and 14999 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cfg/active_grasp.yaml
View File

@ -1,9 +1,9 @@
bt_sim: bt_sim:
gui: True gui: True
gripper_force: 100 gripper_force: 100
scene: random # scene: random
# scene: manual # scene: manual
# scene: $(find active_grasp)/cfg/sim/challenging_scene_4.yaml scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml
hw: hw:
roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
@ -47,4 +47,5 @@ ap_grasp:
crop_radius_step: 0.05 crop_radius_step: 0.05
crop_max_radius: 0.5 crop_max_radius: 0.5
num_knn_neighbours: 5 num_knn_neighbours: 5
gsnet_grasp_offset: [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.01], [0, 0, 0, 1]]

29992
gsnet_input_points.txt
View File

File diff suppressed because it is too large Load Diff

1
src/active_grasp/active_perception_policy.py
View File

@ -76,6 +76,7 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
self.pcdvis = RealTime3DVisualizer() self.pcdvis = RealTime3DVisualizer()
self.updated = False self.updated = False
self._base_url = flask_base_url self._base_url = flask_base_url
self.grasp_net_type = 'gsnet'
# For debugging # For debugging
self.pcd_publisher = rospy.Publisher('/debug_pcd', PointCloud2, queue_size=10) self.pcd_publisher = rospy.Publisher('/debug_pcd', PointCloud2, queue_size=10)

8
src/active_grasp/controller.py
View File

@ -38,6 +38,8 @@ class GraspController:
self.linear_vel = rospy.get_param("~linear_vel") self.linear_vel = rospy.get_param("~linear_vel")
self.move_to_target_threshold = rospy.get_param("~move_to_target_threshold") 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")
self.gsnet_grasp_offset = np.asarray(rospy.get_param("ap_grasp/gsnet_grasp_offset"))
self.gsnet_grasp_offset = Transform.from_matrix(self.gsnet_grasp_offset)
def init_service_proxies(self): def init_service_proxies(self):
self.reset_env = rospy.ServiceProxy("reset", Reset) self.reset_env = rospy.ServiceProxy("reset", Reset)
@ -177,6 +179,9 @@ class GraspController:
return np.r_[linear, angular] return np.r_[linear, angular]
def execute_grasp(self, grasp): def execute_grasp(self, grasp):
if self.policy.grasp_net_type == "gsnet":
self.T_grasp_ee = self.T_grasp_ee * self.gsnet_grasp_offset
self.create_collision_scene() self.create_collision_scene()
T_base_grasp = self.postprocess(grasp.pose) T_base_grasp = self.postprocess(grasp.pose)
self.gripper.move(0.08) self.gripper.move(0.08)
@ -186,7 +191,8 @@ class GraspController:
self.moveit.scene.clear() self.moveit.scene.clear()
self.moveit.execute(plan) self.moveit.execute(plan)
rospy.sleep(0.5) # Wait for the planning scene to be updated rospy.sleep(0.5) # Wait for the planning scene to be updated
self.moveit.gotoL(T_base_grasp * self.T_grasp_ee) grasp_pose = T_base_grasp * self.T_grasp_ee
self.moveit.gotoL(grasp_pose)
rospy.sleep(0.5) rospy.sleep(0.5)
self.gripper.grasp() self.gripper.grasp()
T_base_retreat = Transform.t_[0, 0, 0.05] * T_base_grasp * self.T_grasp_ee T_base_retreat = Transform.t_[0, 0, 0.05] * T_base_grasp * self.T_grasp_ee

1
src/active_grasp/policy.py
View File

@ -24,6 +24,7 @@ class Policy:
self.load_parameters() self.load_parameters()
self.init_ik_solver() self.init_ik_solver()
self.init_visualizer() self.init_visualizer()
self.grasp_net_type = 'vgn'
def load_parameters(self): def load_parameters(self):
self.base_frame = rospy.get_param("~base_frame_id") self.base_frame = rospy.get_param("~base_frame_id")