This is what I have computed straight from Matlab using robotics toolbox and symbolic toolbox for your manipulator, just so you can see the problem with the implementation. The major issue is to make a library for 6DOF manipulators that is general in a sense that this example below only works for the manipulator I derived from DH convention of your manipulator, and still I cannot guarantee that it will work even then.

When you build or buy a robotic arm with 6DOF, you derive the kinematics for that arm specifically. The kinematics will differ from other 6DOF manipulators.

The homogenous transform matrix describing the orientation and position of the end effector based on the given joint angles q1 q2 q3 q4 q5 and q6 and link lengths (from "shoulder" to "elbow" to "wrist" to end effector) l1 l2 l3 and l4 is huge for a 6DOF manipulator, too big for the forum to allow me to paste it here (exceeds 9000 characters).

From the last column in the Homogenous Transform Matrix, you get the x, y, z position as:

```
x = l4*(cos(q4)*sin(q1) - sin(q4)*(cos(q1)*sin(q2 + pi/2)*sin(q3 - pi/2) - cos(q1)*cos(q2 + pi/2)*cos(q3 - pi/2))) - l3*(cos(q1)*cos(q2 + pi/2)*sin(q3 - pi/2) + cos(q1)*cos(q3 - pi/2)*sin(q2 + pi/2)) + l2*cos(q1)*cos(q2 + pi/2);
y = l2*cos(q2 + pi/2)*sin(q1) - l3*(cos(q2 + pi/2)*sin(q1)*sin(q3 - pi/2) + cos(q3 - pi/2)*sin(q1)*sin(q2 + pi/2)) - l4*(cos(q1)*cos(q4) - sin(q4)*(cos(q2 + pi/2)*cos(q3 - pi/2)*sin(q1) - sin(q1)*sin(q2 + pi/2)*sin(q3 - pi/2)));
z = l1 + l3*(cos(q2 + pi/2)*cos(q3 - pi/2) - sin(q2 + pi/2)*sin(q3 - pi/2)) + l2*sin(q2 + pi/2) + l4*sin(q4)*(cos(q2 + pi/2)*sin(q3 - pi/2) + cos(q3 - pi/2)*sin(q2 + pi/2));
```

Unfortunately, you will have to practice your maths skills if you want to implement anything like this, or wait until someone releases an appropriate library for this if you really want to do this in Arduino. There are solutions that can be used in Robotics Toolbox in Matlab that I would recommend. You can then run the kinematics calculations on your PC and communicate with your Arduino through Serial to move the joints.

This is the MATLAB code I used (Robotics toolbox and Symbolic toolbox required):

```
syms l1 l2 l3 l4 q1 q2 q3 q4 q5 q6 real
L(1)=Revolute('d', l1, 'a', 0, 'alpha', pi/2);
L(2)=Revolute('d', 0, 'a', l2, 'alpha', 0);
L(3)=Revolute('d', 0, 'a', 0, 'alpha', -pi/2);
L(4)=Revolute('d', l3, 'a', 0, 'alpha', pi/2);
L(5)=Revolute('d', 0, 'a', 0, 'alpha', pi/2);
L(6)=Revolute('d', l4, 'a', 0, 'alpha', 0);
AngleOffset=[0 pi/2 -pi/2 0 pi 0];
r=SerialLink(L,'name','6DOF Manipulator Arm','offset',AngleOffset);
T = r.fkine([q1 q2 q3 q4 q5 q6]);
disp(vpa(T,2))
```

I will look into this because of my own personal interest, maybe I will create a library for it using matrix math, we'll see.