Создать задачу (Generate Task) - траектория пространства

Вычислите с помощью интерполяции ППМ траектории задачи-пространства.

Сначала вычислите расстояние перемещения инструмента.

distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

Затем определите время траектории на основе расстояния перемещения и требуемой скорости инструмента.

initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime:timeStep:finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];

Интерполяция между taskInit и taskFinal для вычисления промежуточных ППМ пространства задач.

[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes); 

Задача управления - космическое движение

Создайте модель движения пространства соединения для управления PD на соединениях. taskSpaceMotionModel объект моделирует движение модели дерева жесткого тела под управлением пропорциональной производной пространства задачи.

tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

Установите пропорциональный и производный выигрыши при ориентации на ноль, так чтобы управляемое поведение просто следовало ссылочным позициям:

tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;

Определите начальные состояния (положения соединения и скорости).

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

Использовать ode15s для моделирования движения робота. Для этой проблемы замкнутая система является жесткой, что означает, что существует разница в масштабировании где-то в проблеме. В результате интегратор иногда вынужден делать чрезвычайно маленькие шаги, а нежесткий решатель ОДУ, такой как ode45 на решение той же проблемы потребуется гораздо больше времени. Дополнительные сведения см. в разделе Выбор решателя ОДУ и решение жестких ОДУ в документации.

Поскольку опорное состояние изменяется в каждый момент времени, требуется функция обертки для обновления интерполированной траектории, введенной в производную состояния в каждый момент времени. Поэтому пример вспомогательной функции передается в качестве дескриптора функции решателю ОДУ. Результирующие состояния манипулятора выводятся в stateTask.

[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);

Создать траекторию соединения-пространства

Создайте объект обратной кинематики для робота.

ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];

Рассчитайте начальную и требуемую конфигурации соединений с помощью обратной кинематики. Оберните значения в pi, чтобы убедиться, что интерполяция находится в минимальной области.

initialGuess = jointInit;
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);

По умолчанию решение IK учитывает пределы соединения. Однако для непрерывных соединений (вращающихся соединений с бесконечным диапазоном) результирующие значения могут быть излишне большими и могут быть обернуты в [-pi, pi] для обеспечения того, чтобы конечная траектория охватывала минимальное расстояние. Поскольку прерывистые соединения для Gen3 уже имеют пределы в пределах этого интервала, достаточно просто обернуть значения соединений в pi. Непрерывные соединения будут сопоставлены с интервалом [-pi, pi]и другие значения останутся прежними.

wrappedJointFinal = wrapToPi(jointFinal);

Интерполяция между ними с использованием кубической полиномиальной функции для создания массива равномерно разнесенных конфигураций соединений. Используйте B-сплайн для создания гладкой траектории.

ctrlpoints = [jointInit',wrappedJointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

Траектория управляющего соединения и пространства

Создайте модель движения пространства соединения для управления PD на соединениях. jointSpaceMotionModel объект моделирует движение модели дерева жесткого тела и использует пропорционально-производное управление для заданных положений соединения.

jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

Установите начальные состояния (положения и скорости соединения).

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

Использовать ode15s для моделирования движения робота. Опять же, пример вспомогательной функции используется в качестве дескриптора функции, вводимого в решатель ОДУ для обновления опорных входных данных в каждый момент времени. Состояния совместного пространства выводятся в stateJoint.

[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);

Визуализация и сравнение траекторий роботов

Отображение начальной конфигурации робота.

show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');
hold on
axis([-1 1 -1 1 -0.1 1.5]);

Визуализация траектории «задача-пространство». Выполнить итерацию через stateTask состояния и интерполяция на основе текущего времени.

for i=1:length(trajTimes)
    % Current time 
    tNow= trajTimes(i);
    % Interpolate simulated joint positions to get configuration at current time
    configNow = interp1(tTask,stateTask(:,1:numJoints),tNow);
    poseNow = getTransform(robot,configNow,endEffector);
    show(robot,configNow,'PreservePlot',false,'Frames','off');
    taskSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20);
    drawnow;
end

Визуализируйте траекторию соединения с пространством. Выполнить итерацию через jointTask состояния и интерполяция на основе текущего времени.

% Return to initial configuration
show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');

for i=1:length(trajTimes)
    % Current time 
    tNow= trajTimes(i);
    % Interpolate simulated joint positions to get configuration at current time
    configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow);
    poseNow = getTransform(robot,configNow,endEffector);
    show(robot,configNow,'PreservePlot',false,'Frames','off');
    jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20);
    drawnow;
end

% Add a legend and title
legend([taskSpaceMarker jointSpaceMarker], {'Defined in Task-Space', 'Defined in Joint-Space'});
title('Manipulator Trajectories')