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

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

Во-первых, вычислите расстояние путешествующего инструмента.

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

Затем задайте времена траектории на основе путешествования на расстояние и желаемую скорость инструмента.

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

Интерполируйте между taskInit и taskFinal вычислить промежуточный пробел задачи waypoints.

[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 симулировать движение робота. Начиная со ссылочных изменений состояния в каждый момент функция обертки требуется, чтобы обновлять интерполированный вход траектории к производной состояния в каждый момент. Поэтому функция помощника в качестве примера передается как указатель на функцию решателю ОДУ. Результирующие состояния манипулятора выводятся в 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];

Вычислите начальные и желаемые объединенные настройки с помощью инверсной кинематики.

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

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

ctrlpoints = [jointInit',jointFinal'];
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');
    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');
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20)
    drawnow;
end