fixed transformation to WristPoint under condition_angle;
known bugs: inverse kinematic is wrong in calculation of theta_1, irgendein denkfehler! müssen wir mal per hand durchrechnen an nem Beispiel!