Inverse kinematics robotic arm 6dof

Hello
I am trying to control a 6dof robotic arm with an old controller and would need to calculate the 6 joint rotation after inputting a target x,y,z position and rotation.
So far from my research the inverse kinematics topic looks wide and complex . I am looking for some recommendation and place to start or maybe some existing example in OF
Thanks!

I started a simulation to control the arm rotations (I started with trigonometry and then realized using quaternions was much simpler) this is what I have so far

robot.PNG1028x768 53.2 KB

det_181_Kuka_Kr6_work_area_eurobots_1.jpg716x634 46.6 KB

bone1Width = 300;
boneA3Height = 155;

boneLenght[0] = 675;
boneLenght[1] = 650;
boneLenght[2] = 0;
boneLenght[3] = 600;
boneLenght[4] = 125;
boneLenght[5] = 0;

ofVec3f anchor;
ofVec3f newPt;
ofVec3f dir(0, 0, 0);

jointQuat[0].makeRotate(jointRot[0], 0, 1, 0);

anchor = ofVec3f(0, boneLenght[0], bone1Width);
jointPos[0] = anchor * jointQuat[0];

jointQuat[1].makeRotate(jointRot[1], 1, 0, 0);
jointPos[1] = ofVec3f(0, 0, boneLenght[1]) + jointPos[0];
anchor = jointPos[0];
dir = jointPos[1] - anchor;
dir = dir  * jointQuat[1] * jointQuat[0];
jointPos[1] = dir + anchor;

jointQuat[2].makeRotate(jointRot[2], 1, 0, 0);
jointPos[2] = ofVec3f(0, 0, boneLenght[2]) + jointPos[1];
anchor = jointPos[1];
dir = jointPos[2] - anchor;
dir = dir  * jointQuat[2] * jointQuat[1] * jointQuat[0];
jointPos[2] = dir + anchor;


uppoint = ofVec3f(0, boneA3Height, 0) + jointPos[2];
anchor = jointPos[2];
dir = uppoint - anchor;
dir = dir * jointQuat[2] * jointQuat[1] * jointQuat[0];
uppoint = dir + anchor;

jointQuat[3].makeRotate(jointRot[3], 0, 0, 1);
jointPos[3] = ofVec3f(0, 0, boneLenght[3]) + uppoint;
anchor = uppoint;
dir = jointPos[3] - anchor;
dir = dir  *  jointQuat[3] * jointQuat[2] * jointQuat[1] * jointQuat[0];
jointPos[3] = dir + anchor;


jointQuat[4].makeRotate(jointRot[4], 1, 0, 0);
jointPos[4] = ofVec3f(0, 0, boneLenght[4]) + jointPos[3];
anchor = jointPos[3];
dir = jointPos[4] - anchor;
dir = dir  *  jointQuat[4] * jointQuat[3] * jointQuat[2] * jointQuat[1] * jointQuat[0];
jointPos[4] = dir + anchor;



jointQuat[5].makeRotate(jointRot[5], 0, 0, 1);
jointPos[5] = ofVec3f(0, 0, boneLenght[5]) + jointPos[4];
anchor = jointPos[4];
dir = jointPos[5] - anchor;
dir = dir *  jointQuat[5] * jointQuat[4] * jointQuat[3] * jointQuat[2] * jointQuat[1] * jointQuat[0];
jointPos[5] = dir + anchor;

posFinal = ofVec3f(70, 100, 150) + jointPos[5];
anchor = jointPos[5];
dir = posFinal - anchor;
dir = dir *  jointQuat[5] * jointQuat[4] * jointQuat[3] * jointQuat[2] * jointQuat[1] * jointQuat[0];// *;//
posFinal = dir + anchor;