Прототип ноги робота: обратная кинематика

Просидел над этим примерно четыре вечера, но все-таки сделал то, что хотел.

Теперь конец ноги способен правильно позиционироваться относительно начала ноги зная координаты конца ноги. Не очень красиво звучит. :) Ну, в общем, на видео, наверное, будет понятнее: