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 close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() rospy.sleep(1) grasp_status = self.get_grasp_status_srv() return grasp_status.is_hand_occupied
def close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() rospy.sleep(1) grasp_status = self.get_grasp_status_srv() return grasp_status.is_hand_occupied
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)
def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE result = self.posture_controller.send_goal_and_wait(pg) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to open gripper') return False return True
def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE result = self.posture_controller.send_goal_and_wait(pg) if result != GoalStatus.SUCCEEDED: rospy.logerr('failed to open gripper') return False return True
def gentle_close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.CLOSE_GRIPPER goal.torque_limit = 0.0 goal.dynamic_torque_control = False self.gripper_controller.send_goal(goal) self.gripper_controller.wait_for_result()
def gentle_close_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result() goal = WubbleGripperGoal() goal.command = WubbleGripperGoal.CLOSE_GRIPPER goal.torque_limit = 0.0 goal.dynamic_torque_control = False self.gripper_controller.send_goal(goal) self.gripper_controller.wait_for_result()
def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result()
def open_gripper(self): pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE self.posture_controller.send_goal(pg) self.posture_controller.wait_for_result()
import roslib; roslib.load_manifest('wubble2_robot') import rospy from actionlib import SimpleActionClient from object_manipulation_msgs.msg import GraspHandPostureExecutionAction from object_manipulation_msgs.msg import GraspHandPostureExecutionGoal if __name__ == '__main__': rospy.init_node('test_gripper_posture_controller', anonymous=True) posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction) posture_controller.wait_for_server() rospy.loginfo('got gripper_posture_controller') pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.RELEASE posture_controller.send_goal(pg) posture_controller.wait_for_result() rospy.loginfo('finished releasing') pg = GraspHandPostureExecutionGoal() pg.goal = GraspHandPostureExecutionGoal.GRASP posture_controller.send_goal(pg) posture_controller.wait_for_result() rospy.loginfo('finished grasping')