## 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.

### 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()