added FK/IK and velocityIK test. Not working