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

Check whether grasp can be reached

Browse Source
This commit is contained in:
Michel Breyer 2021-11-08 13:13:43 +01:00
parent c82e7bde50
commit 9fccea5681
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/active_grasp/controller.py
View File

@ -137,14 +137,18 @@ class GraspController:
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)
self.moveit.goto(T_base_grasp * Transform.t([0, 0, -0.06]) * self.T_grasp_ee) T_base_approach = T_base_grasp * Transform.t([0, 0, -0.06]) * self.T_grasp_ee
self.remove_target_collision_object(grasp) success, plan = self.moveit.plan(T_base_approach)
rospy.sleep(0.5) # Wait for the planning scene to be updated if success:
self.moveit.gotoL(T_base_grasp * self.T_grasp_ee) self.moveit.execute(plan)
self.gripper.grasp() self.remove_target_collision_object(grasp)
self.moveit.gotoL(Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee) rospy.sleep(0.5) # Wait for the planning scene to be updated
rospy.sleep(1.0) # Wait to see whether the object slides out of the hand self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
success = self.gripper.read() > 0.002 self.gripper.grasp()
T_base_retreat = Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee
self.moveit.gotoL(T_base_retreat)
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" return "succeeded" if success else "failed"
def create_collision_scene(self): def create_collision_scene(self):