С помощью программных элементов библиотеки MATLAB Simulink собрать математическую модель системы манипулятора, либо написать сам код. То есть либо с помощью блоков, либо код.
Самое главное получить саму модель в конце.
И самое важное, все должно быть в декартовой системе координат.