Запланируйте и выполните задачу - и Объединенные Пространственные траектории Используя манипулятор КИНОВОЙ Gen3

В этом примере показано, как сгенерировать и симулировать интерполированные объединенные траектории, чтобы переместиться от начальной буквы до желаемого положения исполнительного элемента конца. Синхронизация траекторий основана на аппроксимированной желаемой скорости конца инструмента руки (EOAT).

Загрузите модель робота дерева твердого тела (RBT) КИНОВОЙ Gen3.

robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

Установите текущую настройку соединения робота.

currentRobotJConfig = homeConfiguration(robot);

Получите количество соединений и исполнительного элемента конца система координат RBT.

numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";

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

timeStep = 0.1; % seconds
toolSpeed = 0.1; % m/s

Установите начальное и итоговое положение исполнительного элемента конца.

jointInit = currentRobotJConfig;
taskInit = getTransform(robot,jointInit,endEffector);

taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

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

Вычислите пространственную траекторию задачи 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 симулировать движение робота. Для этой проблемы система с обратной связью жестка, означая, что существует различие в масштабировании где-нибудь в проблеме. В результате интегратор иногда обеспечивается, чтобы сделать чрезвычайно небольшие шаги и нежесткий решатель ОДУ, такие как ode45 возьмет намного дольше, чтобы решить ту же задачу. Для получения дополнительной информации относитесь, чтобы Выбрать ODE Solver и Solve Stiff ODEs в документации.

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

Figure contains an axes. The axes with title Manipulator Trajectories contains 150 objects of type line, patch. These objects represent Defined in Task-Space, base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Defined in Joint-Space.

Для просмотра документации необходимо авторизоваться на сайте