Calculating Forward/Inverse Kinematics for 4-DOF Manipulators
Enter the desired values for:
px, py, pz - End-effector coordinates;
a1, a2 - Link lengths;
tool_angle - Angle of the tool (possibly gripper) with respect to base frame;
tcp - Tool Center Point;
and get two sets of possible joint variables.
Change the DH parameters according to your robot's configurations.
a1 = 398.5 # height from frame 0 to frame 1
a2 = 320.6 # distance from frame 0 to frame 1 in X0 direction ; also length of link 1
a3 = 320.0 # distance from frame 1 to frame 2 in X1 direction ; also length of link 2
a4 = 173.0 # height from frame 1 to frame 2
a5 = 294.0 # height from frame 2 to frame 3
alpha = [np.radians(0), np.radians(180), np.radians(0), np.radians(0)]
r = [a2, a3, 0, 0]
d = [a1, a4, a5 + stroke, 0]
Enter the desired values for joint variables:
th1 = np.radians(0.0)
th2 = np.radians(0.0)
th4 = np.radians(0.0)
stroke = 0.0
This will print the transformation matrix showing the projection of the end-effector frame to the base frame:
print(np.round(H0_4, 3))