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


based on this robot arm
this is the code for the simulation so far

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;

i would try using ofNode for each joint, that way you can chain all the local transformations without having to do all the calculations manually. The rotations are internally expressed in quaternions in ofNode so it’s really the same but you can parent one node to another instead of having to do all the multiplications manually