Need help with last part Inverse Kinematic 6DoF

[Mr Jos]

Hey,

 

Days in row I've been breaking my head over formulas. I managed to get the forward kinematics working (finding current X/Y/Z coördinates for the endpiece on the 6DoF arm. [This tiny bit of manual multiplying matrix's]

Resulting in the code [This photo has some - and + switched up, but solved now];

Then I started on the inverse kinematics; I managed to get formulas for 1st, 2nd and 3rd rotation joint and finding wrist X/Y/Z position by giving in only: End position for endpiece in X/Y/Z and it's roll/pitch/yaw.

But I'm stuck now. No mather how many videos I look at to understand, I just don't get it how I can solve it to find angles for joint 4/5/6. I need to use some rotation matrix I think, but whatever I try I don't find the correct answer.

Then even started just making some spreadsheet to see if I can find some "easy" formula, but nope.

So any got an idea what last bit I need to get my last angles calculated?