def preempt_callback(self): rospy.logwarn("Point action got cancelled!!") self.cancelled = True self._result.result = -1 yumi.reset_pose() self.server.set_preempted(self._result)
def run(planning_frame="/world"): """Starts the node Runs to start the node and initialize everthing. :returns: Nothing :rtype: None """ #Start by connecting to ROS and MoveIt! yumi.init_Moveit(planning_frame) # Print current joint angles rospy.loginfo("Printing right arm joint states...") yumi.print_current_joint_states(yumi.RIGHT) rospy.loginfo("Printing left arm joint states...") yumi.print_current_joint_states(yumi.LEFT) # Drive YuMi end effectors to a desired position (pose_ee_x), and perform a grasping task with a given effort (grip_effort) # Gripper effort: opening if negative, closing if positive, static if zero # Default pose # Right arm Target pose pose_ee_r = [0.3, -0.3, 0.25, 0.0, 3.14, 0.0] yumi.open_grippers(yumi.LEFT) yumi.open_grippers(yumi.RIGHT) yumi.move_global_planning(yumi.RIGHT, pose_ee_r) rospy.sleep(2.0) pose_ee_r[2] = 0.2 yumi.move_global_planning(yumi.RIGHT, pose_ee_r) rospy.sleep(2.0) pose_ee_r[2] = 0.25 yumi.move_global_planning(yumi.RIGHT, pose_ee_r) rospy.sleep(2.0) pose_ee_l = [0.3, 0.3, 0.25, 0.0, 3.14, 0.0] yumi.move_global_planning(yumi.LEFT, pose_ee_l) rospy.sleep(2.0) pose_ee_l[2] = 0.2 yumi.move_global_planning(yumi.LEFT, pose_ee_l) rospy.sleep(2.0) pose_ee_l[2] = 0.25 yumi.move_global_planning(yumi.LEFT, pose_ee_l) rospy.sleep(2.0) # Reset YuMi joints to "home" position yumi.reset_pose()
def run(): """Starts the node Runs to start the node and initialize everthing. Runs forever via Spin() :returns: Nothing :rtype: None """ rospy.init_node('moveit_yumi_resetpose') # Start by connecting to ROS and MoveIt! yumi.init_Moveit("/world") # Print current joint angles yumi.print_current_joint_states(yumi.RIGHT) yumi.print_current_joint_states(yumi.LEFT) # Reset YuMi joints to "home" position yumi.reset_pose()
def execute(self, goal): self.cancelled = False hand_id = -1 # Close the hand gripper grip_effort = 0.0 close_grippers(yumi.LEFT) yumi.gripper_effort(yumi.LEFT, 0.0) close_grippers(yumi.RIGHT) yumi.gripper_effort(yumi.RIGHT, 0.0) #Reset YuMi joints to "home" position yumi.reset_pose() if not self.cancelled: self._result.result = 0 self.server.set_succeeded(self._result)
def abort(self): yumi.reset_pose() self._result.result = -2 self.server.set_aborted(self._result) self.cancelled = True
def execute(self, goal): self.cancelled = False hand_id = -1 if goal.location.position.y > 0.0: hand_id = yumi.LEFT else: hand_id = yumi.RIGHT # Open the grippers initially grip_effort = -10.0 open_grippers(yumi.LEFT) open_grippers(yumi.RIGHT) pose_ee_t = [goal.location.position.x, goal.location.position.y, 0.3, 0.0,3.14, 0.0] if not self.cancelled: if not move_and_grasp(hand_id, pose_ee_t, grip_effort): self.abort() return rospy.sleep(2.0) pose_ee_t[2] = 0.20 pose_ee_t[5] = goal.location.orientation.z#-3.14159 if not self.cancelled: if not move_and_grasp(hand_id, pose_ee_t, grip_effort): self.abort() return rospy.sleep(2.0) close_grippers(hand_id) pose_ee_t[2] = 0.4 if not self.cancelled: if not move_and_grasp(hand_id, pose_ee_t, grip_effort): self.abort() return rospy.sleep(2.0) if not self.cancelled: if hand_id == yumi.LEFT: pose_ee = [0.1, 0.4, 0.3, 0.0, -2.0, 0.0] grip_effort = 10.0 if not move_and_grasp(hand_id, pose_ee, grip_effort): self.abort() return rospy.sleep(2.0) open_grippers(hand_id) #rospy.sleep(2.0) #pose_ee = [0.1, 0.4, 0.3, 0.0, 3.14, 0.0] #move_and_grasp(hand_id, pose_ee, grip_effort) else: pose_ee = [0.1, -0.4, 0.3, 0.0, -2.0, 0.0] if not move_and_grasp(hand_id, pose_ee, grip_effort): self.abort() return rospy.sleep(2.0) open_grippers(hand_id) #rospy.sleep(2.0) #pose_ee = [0.1, -0.4, 0.3, 0.0, 3.14, 0.0] #move_and_grasp(hand_id, pose_ee, grip_effort) #Reset YuMi joints to "home" position yumi.reset_pose() if not self.cancelled: self._result.result = 0 self.server.set_succeeded(self._result)