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 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 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 unplug_connector(self, grasp): #We compute a list of poses to send to # the arm (going up from grasp) rospy.loginfo("unplugging the connector") goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = grasp.grasp_pose pose_tmp.position.z += 0.1 traj.append(pose_tmp) speed.append(30.) 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 rospy.loginfo("unplugged the connector") #Move the arm to a fixed position (above the box where the connector will be dropped) goal = TrajectoryGoal() traj = [] speed = [] pose_above_box = Pose() pose_above_box.position.x = 0.454051 pose_above_box.position.y = 0.283831 pose_above_box.position.z = 0.296853 pose_above_box.orientation.x = 0.711336 pose_above_box.orientation.y = -0.31226 pose_above_box.orientation.z = 0.577 pose_above_box.orientation.w = 0.25212 traj.append(pose_above_box) speed.append(100.) #pose_in_box = Pose() #pose_in_box.position.x = 0.561363 #pose_in_box.position.y = 0.229195 #pose_in_box.position.z = 0.228286 #pose_in_box.orientation.x = 0.772113 #pose_in_box.orientation.y = -0.318396 #pose_in_box.orientation.z = 0.532802 #pose_in_box.orientation.w = 0.198744 #traj.append( pose_in_box ) #speed.append( 10. ) goal.trajectory = traj goal.speed = speed rospy.loginfo("Putting the connector in the box") 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 release the object goal = GraspHandPostureExecutionGoal() goal.grasp = grasp rospy.loginfo("Going to release the object") goal.goal = goal.RELEASE 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 Release") return False time.sleep(0.1) rospy.loginfo("Released the connector") 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 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')