Сгенерируйте траекторию пространства задач

Вычислите точки пути траектории пространства задач посредством интерполяции.

Во-первых, вычислите дорожное расстояние инструмента.

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 в соединениях. The 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 в соединениях. The 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')