Python go_to_joints Examples

Example #1

File: main.py Project: joker0100/URP

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)

Example #2

File: main.py Project: joker0100/URP

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)

Example #3

File: main.py Project: joker0100/URP

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)

Example #4

File: main.py Project: joker0100/URP

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)

Example #5

File: main.py Project: joker0100/URP

            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