Продолжаю рыть в этом вопросе. Я так понимаю без обратной кинематики тут не обойтись. Нашел по теме хорошую статью.
http://roboty6.narod.ru/inverseKinematics.htm - про обратную и прямую кинематику.
В Блице накидал формулы но что то не пойму, не работает как надо, вроде и в градусы переводил и менял центры расчета, а у меня либо зеркальное либо кривое отражение результата. Если кто захочет помочь выложу код.