def cup_from_front():
yumi.go_to_simple(0.2, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.go_to_simple(0.25, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_simple(0.55, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def cup_from_top():
yumi.go_to_simple(0.37, 0.2, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.go_to_simple(0.37, 0.2, 0.25, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_simple(0.67, 0.2, 0.25, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def test_200518_v2():
yumi.go_to_simple(0.25, 0.1, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.go_to_simple(0.25, 0.2, 0.15, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(giving_point_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.25, 0.1, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.go_to_simple(0.25, 0.1, 0.15, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(giving_point_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def test_200513():
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_simple(0.25, 0.0, 0.2, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.6, 0.0, 0.2, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.25, 0.1, 0.2, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.6, 0.0, 0.2, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.25, 0.2, 0.2, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.6, 0.0, 0.2, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
if (direction == 1):
cup_from_left()
elif (direction == 2):
cup_from_top()
elif (direction == 3):
cup_from_front()
else:
continue
else:
continue
if __name__ == '__main__':
try:
# run()
rospy.init_node('yumi_moveit_demo')
#Start by connecting to ROS and MoveIt!
yumi.init_Moveit()
# yumi.go_to_simple(0.55, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.go_to_simple(0.2, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
# Giving joints: (0.67, 0.2, 0.3, 1.57, 0, 1.57, yumi.LEFT)
# yumi.go_to_joints(left_base_angle, yumi.LEFT)
# main_test()
# test_action_with_traverse()
print "#################################### Program finished ####################################"
except rospy.ROSInterruptException:
pass