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

Approach grasp and lift with Cartesian planner

Browse Source
This commit is contained in:
Michel Breyer 2021-10-13 13:55:18 +02:00
parent 8a1b8130dc
commit 3cc4fb159b
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/active_grasp/controller.py
View File

@ -165,11 +165,12 @@ class GraspController:
T_base_grasp = self.postprocess(grasp.pose)
self.gripper.move(0.08)
self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.05]) * self.T_grasp_ee)
self.moveit.scene.attach_mesh("panda_hand", self.target_co_name)
rospy.sleep(0.5)
self.moveit.goto(T_base_grasp * self.T_grasp_ee)
self.moveit.scene.remove_world_object(self.target_co_name)
rospy.sleep(0.5) # Wait for the planning scene to be updated
self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
self.gripper.grasp()
self.moveit.goto(Transform.t([0, 0, 0.1]) * T_base_grasp * self.T_grasp_ee)
self.moveit.gotoL(Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee)
rospy.sleep(1.0) # Wait to see whether the object slides out of the hand
success = self.gripper.read() > 0.002
return "succeeded" if success else "failed"