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

8
src/active_grasp/controller.py
View File

@ -137,12 +137,16 @@ 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
success, plan = self.moveit.plan(T_base_approach)
if success:
self.moveit.execute(plan)
self.remove_target_collision_object(grasp) self.remove_target_collision_object(grasp)
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) self.moveit.gotoL(T_base_grasp * self.T_grasp_ee)
self.gripper.grasp() self.gripper.grasp()
self.moveit.gotoL(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
self.moveit.gotoL(T_base_retreat)
rospy.sleep(1.0) # Wait to see whether the object slides out of the hand rospy.sleep(1.0) # Wait to see whether the object slides out of the hand
success = self.gripper.read() > 0.002 success = self.gripper.read() > 0.002
return "succeeded" if success else "failed" return "succeeded" if success else "failed"