There were some bugs in the algo + rename framesForwardKinematic in framesForwardKinematics (follow the API)