def grab(): yumi.gripper_effort(yumi.LEFT, -10) #yumi.go_to_simple(0.4, 0.2, 0.03, 0, 3.14, 0, yumi.LEFT) #yumi.go_to_simple(0.4, 0.2, 0.03, 3.14, 0, 0, yumi.LEFT) yumi.go_to_simple(0.4, 0.2, 0.3, 0, 3, 0, yumi.LEFT) yumi.go_to_simple(0.4, 0.2, 0.03, 0, 3.14, 0, yumi.LEFT) yumi.gripper_effort(yumi.LEFT, 20)
def test_action_with_traverse(): yumi.group_l.set_max_velocity_scaling_factor(1) yumi.go_to_simple(0.3, 0.1, 0.3, 0, 3.14, 0, yumi.LEFT) yumi.gripper_effort(yumi.LEFT, 10) yumi.gripper_effort(yumi.LEFT, -10) yumi.traverse_path([[0.3, 0.1, 0.3, 0, 3.14, 0], [0.25, 0.0, 0.15, 0, 3.14, 0]], yumi.LEFT) yumi.gripper_effort(yumi.LEFT, 10) yumi.traverse_path([[0.6, 0.1, 0.2, 0, 3.14, 0]], yumi.LEFT)
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)
def give(): #yumi.go_to_simple(0.65, 0.2, 0.2, 0, 1.57, 0, yumi.LEFT) yumi.go_to_simple(0.65, 0.2, 0.2, 0, 1.57, 0, yumi.LEFT)
def check_grip(): yumi.go_to_simple(0.3, 0.3, 0.3, 0, 1.57, 0, yumi.LEFT) yumi.gripper_effort(yumi.LEFT, -20) yumi.gripper_effort(yumi.LEFT, 10)
def pick_up(): yumi.gripper_effort(yumi.LEFT, -20) yumi.go_to_simple(0.5, 0.2, 0.08, 3.14, 0, 0, yumi.LEFT) yumi.gripper_effort(yumi.LEFT, 20) yumi.go_to_simple(0.3, 0.2, 0.3, 1.57, 0, 0, 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