На деревянной копии стоят энкодеры, с которых можно читать текщее положение.
Далее включил режим повтора - крутит копию, манипулятор повторяет.
Таким макаром создал программу - крутил копию, а контроллер записывал на какой угол какой шарнир повернуть и где остановиться. Где остановиться определял на глазок по тому где в данный момент находится манипулятор.
После - выключил режим повтор и запустил режим проигрывания программы по кругу.
По-моему там всё так очевидно.
Добавлено спустя 2 минуты 47 секунд:ЗЫ. не надо дублировать сообщения и плодить темы.