Создание робота

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

Настройка моделирования

Установите временной интервал в 1 с с размером временного интервала 0,01 с. Установите начальное состояние роботов, домашняя конфигурация со скоростью ноль.

tspan = 0:0.01:1;
initialState = [homeConfiguration(robot); zeros(7,1)];

Определите опорное состояние с целевым положением, нулевой скоростью и нулевым ускорением.

targetState = [pi/4; pi/3; pi/2; -pi/3; pi/4; -pi/4; 3*pi/4; zeros(7,1); zeros(7,1)];

Создание модели движения

Моделирование системы с вычисленным контролем крутящего момента и динамикой ошибок, определяемой умеренно быстрым откликом на шаг с перерасходом 5%.

motionModel = jointSpaceMotionModel("RigidBodyTree",robot);
updateErrorDynamicsFromStep(motionModel,.3,.05);

Моделирование робота

Используйте производную функцию модели в качестве входных данных для ode45 для моделирования поведения в течение 1 секунды.

[t,robotState] = ode45(@(t,state)derivative(motionModel,state,targetState),tspan,initialState);

Постройте график ответа

Постройте график положения всех соединений, приводимых в целевое состояние. Соединения с большим смещением между начальным положением и целевым положением приводят в действие к цели с большей скоростью, чем соединения с меньшим смещением. Это приводит к перегрузке, но все суставы имеют одинаковое время отстаивания.

figure
plot(t,robotState(:,1:motionModel.NumJoints));
hold all;
plot(t,targetState(1:motionModel.NumJoints)*ones(1,length(t)),"--");
title("Joint Position (Solid) vs Reference (Dashed)");
xlabel("Time (s)")
ylabel("Position (rad)");