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)