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)