def grasp_connector(self, grasp):
#First we set the hand to the pregrasp position
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
self.grasp_client.send_goal(goal)
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Pregrasp")
return False
self.go_to_middle_2_position()
#then we move the arm to the grasp pose
goal = TrajectoryGoal()
traj = []
speed = []
traj.append(grasp.grasp_pose)
speed.append(45.)
goal.trajectory = traj
goal.speed = speed
self.denso_trajectory_client.send_goal(goal)
self.denso_trajectory_client.wait_for_result()
res = self.denso_trajectory_client.get_result()
if res.val != TrajectoryResult.SUCCESS:
rospy.logerr("Failed to move the arm to the given position.")
return False
#finally we grasp the object
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
rospy.loginfo("Going to grasp")
goal.goal = goal.GRASP
self.grasp_client.send_goal(goal)
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Grasp")
return False
rospy.loginfo("OK grasped")
return True
def grasp_connector(self, grasp):
#First we set the hand to the pregrasp position
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
self.grasp_client.send_goal( goal )
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Pregrasp")
return False
self.go_to_middle_2_position()
#then we move the arm to the grasp pose
goal = TrajectoryGoal()
traj = []
speed = []
traj.append( grasp.grasp_pose )
speed.append( 45. )
goal.trajectory = traj
goal.speed = speed
self.denso_trajectory_client.send_goal( goal )
self.denso_trajectory_client.wait_for_result()
res = self.denso_trajectory_client.get_result()
if res.val != TrajectoryResult.SUCCESS:
rospy.logerr("Failed to move the arm to the given position.")
return False
#finally we grasp the object
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
rospy.loginfo("Going to grasp")
goal.goal = goal.GRASP
self.grasp_client.send_goal( goal )
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Grasp")
return False
rospy.loginfo("OK grasped")
return True
def grasp_exec(self, grasp, timeout_sec=15.0):
hand_posture_goal_ = GraspHandPostureExecutionGoal()
hand_posture_goal_.grasp = grasp
# do grasp action
hand_posture_goal_.goal = GraspHandPostureExecutionGoal.GRASP
hand_posture_goal_.max_contact_force = 1.0
# call hand_posture_exec action lib
rospy.loginfo("Trying to grasp...")
return self.hand_posture_exec(hand_posture_goal_,timeout_sec)
def grasp_release_exec(self,timeout_sec=10.0):
hand_posture_goal_ = GraspHandPostureExecutionGoal()
hand_posture_goal_.grasp = Grasp()
hand_posture_goal_.grasp.pre_grasp_posture.name= [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"]
hand_posture_goal_.grasp.pre_grasp_posture.position = [0]*(18)
# do release action
hand_posture_goal_.goal = GraspHandPostureExecutionGoal.RELEASE
# call hand_posture_exec action lib
rospy.loginfo("Trying to release...")
return self.hand_posture_exec(hand_posture_goal_,timeout_sec)
