'ForwardKinematics.py' ändern

ForwardKinematics.py

@@ -1,28 +1,170 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
-l = [10, 26, 33, 30]
-alpha = np.array([90, 120, -30, -110]) * (np.pi / 180)
+segments_length = [15, 50, 40, 30]
+alpha = np.array([90, 0, 0, 0]) * (np.pi / 180)
+
+
+def calculate_triangle_angles(a, b, c):
+    # Calculate the angles using the law of cosines
+    angle_a = np.rad2deg(np.arccos((b ** 2 + c ** 2 - a ** 2) / (2 * b * c)))
+    angle_b = np.rad2deg(np.arccos((c ** 2 + a ** 2 - b ** 2) / (2 * c * a)))
+    angle_c = 180 - angle_a - angle_b  # The sum of angles in a triangle is 180 degrees
+
+    return angle_a, angle_b, angle_c
+
+
+def calculate_angels_to_horizon(relative_angels_stepper):
+    absolute_angels = [0, 0, 0, 0]
+    absolute_angels[0] = relative_angels_stepper[0]
+    absolute_angels[1] = absolute_angels[0] + relative_angels_stepper[1]
+    absolute_angels[2] = absolute_angels[1] + relative_angels_stepper[2]
+    absolute_angels[3] = absolute_angels[2] + relative_angels_stepper[3]
+
+    for j in range(4):
+        if absolute_angels[j] > 360:
+            absolute_angels[j] = absolute_angels[j] - 360
+    print("relative:", relative_angels_stepper,
+          "absolute:", absolute_angels)
+    return absolute_angels
+
+
+def calculate_angels_relative(absolute_angels_stepper):
+    relative_angels = [0, 0, 0, 0]
+    relative_angels[0] = absolute_angels_stepper[0]
+    relative_angels[1] = absolute_angels_stepper[1] - relative_angels[0]
+    relative_angels[2] = absolute_angels_stepper[2] - relative_angels[1]
+    relative_angels[3] = absolute_angels_stepper[3] - relative_angels[2]
+
+    for j in range(4):
+        if relative_angels[j] > 360:
+            relative_angels[j] = relative_angels[j] - 360
+
+    return relative_angels
+
+
+def ik_calculate_angels(g3_position, g4_orientation):
+    z0 = segments_length[0]
+    # y0 = 0
+
+    alpha = [90, 0, 0, g4_orientation]  # relative stepper angels to reach desired coordination
+    beta = [0, 0, 90]  # angels of right triangle
+
+    y = g3_position[0]  # - y0
+    z = g3_position[1] - z0
+
+    l1 = segments_length[1]
+    l2 = segments_length[2]
+    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis
+
+    gamma = calculate_triangle_angles(l2, l1, l_h)
+
+    beta[0] = np.rad2deg(np.arctan2(z, y))
+    beta[1] = np.rad2deg(np.arctan2(y, z))
+
+    alpha[1] = -(90 - gamma[0] - beta[0])
+    alpha[2] = -(180 - gamma[2])
+
+    return alpha
+
+
+# Wrong!!!!!
+def fk_calculate_position(stepper_angels_rel):
+    alpha = np.array(stepper_angels_rel) * (np.pi / 180)
+    alpha_abs = calculate_angels_to_horizon(alpha)
+    z = [0, 0, 0, 0]
+    y = [0, 0, 0, 0]
+
+    start_x = 0
+    start_y = 0
+
+    z[0] = segments_length[0]
+    y[0] = 0
+
+    z[1] = z[0]
+    y[1] = y[0] + segments_length[1]
+
+    z[2] = y[1] + np.cos(alpha_abs[2]) * segments_length[2]
+    y[2] = z[1] + np.sin(alpha_abs[2]) * segments_length[2]
+
+    z[3] = z[2] + np.cos(alpha_abs[3]) * segments_length[3]
+    y[3] = y[2] + np.sin(alpha_abs[3]) * segments_length[3]
+
+    m4_pos_2d = [y, z]
+    m3_pos_2d = [y, z]
+
+    return m4_pos_2d
+
+
+default_stepper_angels = [90, -90, 0, 0]
+
+
+# Definition of function to calculate stepper angels to move end effector to desired coordination by changing M2 and M3
+def ik_calculate_angels2(end_effector_position):
+    z0 = segments_length[0]
+    y0 = segments_length[1]
+
+    alpha = [90, -90, 0, 0]  # relative stepper angels to reach desired coordination
+    beta = [0, 0, 90]  # angels of right triangle
+
+    y = end_effector_position[0] - y0
+    z = end_effector_position[1] - z0
+
+    l2 = segments_length[2]
+    l3 = segments_length[3]
+    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis
+
+    gamma = calculate_triangle_angles(l3, l2, l_h)
+
+    beta[0] = np.rad2deg(np.arctan2(z, y))
+    beta[1] = np.rad2deg(np.arctan2(y, z))
+
+    alpha[2] = gamma[0] + beta[0]
+    alpha[3] = -(180 - gamma[2])
+
+    return alpha
+
+
+pos = [40, 30]
+alpha_degree = ik_calculate_angels2(pos)
+alpha_rel = np.array(alpha_degree)
+alpha_abs = np.array(calculate_angels_to_horizon(alpha_rel)) * (np.pi / 180)
 
 start_x = 0
 start_y = 0
 
-x0 = np.cos(alpha[0]) * l[0]
-y0 = np.sin(alpha[0]) * l[0]
+x0 = np.cos(alpha_abs[0]) * segments_length[0]
+y0 = np.sin(alpha_abs[0]) * segments_length[0]
+
+x1 = x0 + np.cos(alpha_abs[1]) * segments_length[1]
+y1 = y0 + np.sin(alpha_abs[1]) * segments_length[1]
 
-x1 = x0 + np.cos(alpha[1]) * l[1]
-y1 = y0 + np.sin(alpha[1]) * l[1]
+x2 = x1 + np.cos(alpha_abs[2]) * segments_length[2]
+y2 = y1 + np.sin(alpha_abs[2]) * segments_length[2]
 
-x2 = x1 + np.cos(alpha[2]) * l[2]
-y2 = y1 + np.sin(alpha[2]) * l[2]
+x3 = x2 + np.cos(alpha_abs[3]) * segments_length[3]
+y3 = y2 + np.sin(alpha_abs[3]) * segments_length[3]
 
-x3 = x2 + np.cos(alpha[3]) * l[3]
-y3 = y2 + np.sin(alpha[3]) * l[3]
+# dev_x = [start_x, fk_calculate_position(alpha_degree)[0][0], fk_calculate_position(alpha_degree)[0][1],
+# fk_calculate_position(alpha_degree)[0][2],
+# fk_calculate_position(alpha_degree)[0][3]]
+# dev_y = [start_y, fk_calculate_position(alpha_degree)[1][0], fk_calculate_position(alpha_degree)[1][1],
+# fk_calculate_position(alpha_degree)[1][2],
+# fk_calculate_position(alpha_degree)[1][3]]
 
 dev_x = [start_x, x0, x1, x2, x3]
 dev_y = [start_y, y0, y1, y2, y3]
 
+# Create a figure and axis
+# fig, ax = plt.subplots()
+
+# Set the limits for the x and y axes
+# ax.set_xlim(-130, 130)
+# ax.set_ylim(-130, 130)
+
 plt.plot(dev_x, dev_y, marker='o')
 plt.grid()
+plt.xlabel('Y in cm')
+plt.ylabel('Z in cm')
 
 plt.show()