Program to simulate the forward kinematics of a 3R Robotic Arm

Objective:

1. To write a program to simulate the forward kinematics of a 3R Robotic Arm
2. To create an animation file of the plot

Program:

import math
import matplotlib.pyplot as plt

l1 = 3
l2 = 2
l3 = 1

x0 = 0
y0 = 0

theta_start = 0
theta_end = math.pi/2
n_theta = 7

theta_1 = []

for i in range(0,n_theta):
    tmp_theta = theta_start + ((i * (theta_end - theta_start))/(n_theta - 1))
    theta_1.append(tmp_theta)
    theta_2 = theta_1
    theta_3 = theta_1

'''print(theta_1)
print(theta_2)
print(theta_3)'''
count = 1

for t1 in theta_1:
    for t2 in theta_2:
        for t3 in theta_3:
            x1 = l1 * math.cos(t1)
            y1 = l1 * math.sin(t1)

            x2 = x1 + (l2 * math.cos(t2))
            y2 = y1 + (l2 * math.sin(t2))

            x3 = x2 + (l3 * math.cos(t3))
            y3 = y2 + (l3 * math.sin(t3))

            filename = '%04d.png' %count
            count = count + 1

            plt.figure()
            plt.plot([x0,x1],[y0,y1])
            plt.plot([x1,x2],[y1,y2])
            plt.plot([x2,x3],[y2,y3])
            plt.xlim([-1,7])
            plt.ylim([-1,7])
            plt.savefig(filename)

Explanation:

1. Imported the necessary modules
2. Defined the variables.
3. Used a for loop for creating the list of theta values ranging from 0 to 90 degrees (theta_1 and theta_2 and theta_3)
4. Used for loops to get the values of (x1,y1),(x2,y2) and (x3,y3) for every theta_1,theta_2 and theta_3 in the range(0 to 90 degrees)
5. Used the values of (x1, y1), (x2, y2) and (x3,y3) to plot the robotic arm position at different instances.
6. Saved all the plots in a folder and combined them together in the correct sequence using ImageMagick for creating a gif file showing the motion of the three links of the robotic arm.

Output:

1. The output is the animation of the motion of the robotic arm for theta varying from 0 to 90 degrees.