Roboterarm/ForwardKinematics.py

import numpy as np
import matplotlib.pyplot as plt

l = [10, 26, 33, 30]
alpha = np.array([90, 120, -30, -110]) * (np.pi / 180)

start_x = 0
start_y = 0

x0 = np.cos(alpha[0]) * l[0]
y0 = np.sin(alpha[0]) * l[0]

x1 = x0 + np.cos(alpha[1]) * l[1]
y1 = y0 + np.sin(alpha[1]) * l[1]

x2 = x1 + np.cos(alpha[2]) * l[2]
y2 = y1 + np.sin(alpha[2]) * l[2]

x3 = x2 + np.cos(alpha[3]) * l[3]
y3 = y2 + np.sin(alpha[3]) * l[3]

dev_x = [start_x, x0, x1, x2, x3]
dev_y = [start_y, y0, y1, y2, y3]

plt.plot(dev_x, dev_y, marker='o')
plt.grid()

plt.show()