fk is working, reverse wz manually (ForwardKinematics.m: line 56)