Image

Project 5:Inverse Kinematics

Rui Shi & Ziyi Zhang

Description:

Project 5 asks us to use inverse kinematics to determine end effector positions and control motors to draw through a sequence of points. To add a "lift" and "drop" functionality, based on the project 4 robotic arm, we mounted a gear with two linked rods which can attach pens on it, so that the pen can move up and down by controlling rotation of the gear. Also, we add some other bricks to reinforce the whole arm to ensure it  moves more stable.

Image Image Image Image

Challenge 1:

As we show in the video, our sensor will output the end effector position and then playback. By measure the actual position and the output of sensors, we find out under most occasions the output value is accurate, but sometimes has a small error, for the upper motor might move back a little after we set its position.

Code:

For the first challenge, we keep old code and add calculation part to it. Then we print the angles we get from the calculation and  angles we get from sensor. The result shows that the results are basically same with small deviation.

#!/usr/bin/env python3
import math
def main():
    ev3 = Device('this')
    #Connect the  #1 motor to port A
    #Connect 1the  #2 motor to port B
    m1 = ev3.LargeMotor('outC')
    m2 = ev3.LargeMotor('outA')
    sensor1 = ev3.GyroSensor('in1')
    sensor2 = ev3.GyroSensor('in3')
    sensor1.mode ='GYRO-ANG'
    sensor2.mode ='GYRO-ANG'
    angle1_origin = sensor1.value();
    angle2_origin = sensor2.value();
    l1 = 8
    l2 = 9
    loop = 1
    prev1 = sensor1.value()
    prev2 = sensor2.value() - angle2_origin
    print("Start")
    while loop == 1:
        angle1 = sensor1.value()
        angle2 = sensor2.value() - angle2_origin
        print("angle1 =%d, angle2 =%d" % (angle1, angle2))
        if abs(angle1 - angle1_origin) > 10 and abs(angle2 - angle2_origin) > 10 and abs(prev1 - angle1) < 10 and abs(prev2 - angle2) < 10:
            prev1 = angle1
            prev2 = angle2
            break
        sleep(1)
        prev1 = angle1
        prev2 = angle2
    print("End")
#   calculation 
    theta1 = -prev1
    theta2 = prev2 - prev1
    x = l1 * math.cos(theta1/180*3.14) + l2 * math.cos(-prev2/180*3.14)
    y = l1 * math.sin(theta1/180*3.14) + l2 * math.sin(-prev2/180*3.14)
    theta1o = -prev1
    theta2o = -prev2 + prev1
    r2 = (x**2 + y**2)
    cosa = (l2**2 - (l1**2 + r2))/((-2) * l1 * math.sqrt(r2))
    a = math.atan(math.sqrt(1-cosa**2) / cosa)
    b = math.atan(y/x)
    z = (r2 - l1**2 - l2**2)/(l1 * l2 * (-2))
#   get angle1 and angle2
    if abs(prev2) <= abs(prev1):
        theta1 = a + b
        theta2 = -abs(math.atan(math.sqrt(1 - z**2) / z))
    else:
        theta1 = b - a
        theta2 = abs(math.atan(math.sqrt(1 - z**2) / z))
    theta1_deg = theta1/math.pi * 180
    theta2_deg = theta2/math.pi * 180
#   print origin angle and calculated angle
    print("prev1 =%d, prev2 =%d" % (prev1, prev2))
    print("x =%d, y =%d" % (x, y))
    print("theta1cal =%d, theta2cal =%d" % (theta1_deg, theta2_deg))
    print("theta1orin =%d, theta2orin =%d" % (theta1o, theta2o))
#   run back
    m1.run_to_rel_pos(position_sp=-theta1_deg, speed_sp=200, stop_action="hold")
    m2.run_to_rel_pos(position_sp=-theta2_deg, speed_sp=200, stop_action="hold")
    sleep(1);
#   stop
    m1.stop(stop_action="coast")
    m2.stop(stop_action="coast")
if __name__ == '__main__':
    main()

Challenge 2:

For the second challenge, we build a for loop to get three different positions and record them by pushing them into stack. During one loop, we sense the movement of both part of the arm, then calculate the angle and record them. After finishing all three measurements, we use stack to pop positions to the motor. And we also use a for loop to achieve this function. During the loop, every time the stack pops a position, we drive the motor to rotate back  certain angle to get back to its origin position. To get the right angle, we need to remeasure angle every time we restart to record the rotation.

To make to pen row up before drawing the line and put back when the arm is ready to draw the line, we and another motor on the arm, before we start to record positions, we drive the motor to rotate certain angle and row the pen up. After finishing recording, we drive the motor again in different direction to put the pen back and make it ready to draw.

Code:

#!/usr/bin/env python3
import math
def main():
    ev3 = Device('this')
    #Connect the  #1 motor to port A
    #Connect 1the  #2 motor to port B
    m1 = ev3.LargeMotor('outC')
    m2 = ev3.LargeMotor('outA')
    m3 = ev3.MediumMotor('outB')
    sensor1 = ev3.GyroSensor('in1')
    sensor2 = ev3.GyroSensor('in3')
    sensor1.mode ='GYRO-ANG'
    sensor2.mode ='GYRO-ANG'
    m1.stop(stop_action="coast")
    m2.stop(stop_action="coast")
    m3.stop(stop_action="coast")
    l1 = 8
    l2 = 9
    loop = 1
    positions = []
    m3.run_to_rel_pos(position_sp=50, speed_sp=100, stop_action="hold")
    print("Start")
    for i in range(3):
        print(i)
        angle1_origin = sensor1.value();
        angle2_origin = sensor2.value();
        prev1 = sensor1.value()
        prev2 = sensor2.value() - angle2_origin
        while loop == 1:
            angle1 = sensor1.value() - angle1_origin
            angle2 = sensor2.value() - angle2_origin
            print("angle1 =%d, angle2 =%d" % (angle1, angle2))
            if abs(angle1 - angle1_origin) > 10 and abs(angle2 - angle2_origin) > 10 and abs(prev1 - angle1) < 10   and abs(prev2 - angle2) < 10:
                prev1 = angle1
                prev2 = angle2
                break
            sleep(1)
            prev1 = angle1
            prev2 = angle2
        print("End")
    #   calculation 
        theta1 = -prev1
        theta2 = prev2 - prev1
        x = l1 * math.cos(theta1/180*3.14) + l2 * math.cos(-prev2/180*3.14)
        y = l1 * math.sin(theta1/180*3.14) + l2 * math.sin(-prev2/180*3.14)
        theta1o = -prev1
        theta2o = -prev2 + prev1
        r2 = (x**2 + y**2)
        cosa = (l2**2 - (l1**2 + r2))/((-2) * l1 * math.sqrt(r2))
        a = math.atan(math.sqrt(1-cosa**2) / cosa)
        b = math.atan(y/x)
        z = (r2 - l1**2 - l2**2)/(l1 * l2 * (-2))
    #   get angle1 and angle2
        if abs(prev2) <= abs(prev1):
            theta1 = a + b
            theta2 = -abs(math.atan(math.sqrt(1 - z**2) / z))
        else:
            theta1 = b - a
            theta2 = abs(math.atan(math.sqrt(1 - z**2) / z))
        theta1_deg = theta1/math.pi * 180
        theta2_deg = theta2/math.pi * 180
    #   print origin angle and calculated angle
        print("prev1 =%d, prev2 =%d" % (prev1, prev2))
        print("x =%d, y =%d" % (x, y))
        print("theta1cal =%d, theta2cal =%d" % (theta1_deg, theta2_deg))
        print("theta1orin =%d, theta2orin =%d" % (theta1o, theta2o))
        positions.append((theta1_deg, theta2_deg))

    m3.run_to_rel_pos(position_sp=-50, speed_sp=100, stop_action="hold")
    sleep(1);
    for i in range(3):
        angle1, angle2 = positions.pop();
        m1.run_to_rel_pos(position_sp=-angle1, speed_sp=100, stop_action="hold")
        m2.run_to_rel_pos(position_sp=-angle2, speed_sp=100, stop_action="hold")
        sleep(1);
    m1.stop(stop_action="coast")
    m2.stop(stop_action="coast")
    m3.stop(stop_action="coast")
if __name__ == '__main__':
    main()