Настройте модель робота

Этот пример использует модель KUKA LBR iiwa, робота с 7 степенями свободы. importrobot генерирует rigidBodyTree модель из описания, хранящегося в файле Unified Robot Description Format (URDF).

lbr = importrobot('iiwa14.urdf'); % 14 kg payload version
lbr.DataFormat = 'row';
gripper = 'iiwa_link_ee_kuka';

Задайте размерности для чашки.

cupHeight = 0.2;
cupRadius = 0.05;
cupPosition = [-0.5, 0.5, cupHeight/2];

Добавьте фиксированное тело к модели робота, представляющей центр чашки.

body = rigidBody('cupFrame');
setFixedTransform(body.Joint, trvec2tform(cupPosition))
addBody(lbr, body, lbr.BaseName);

Определите задачу планирования

Цель этого примера состоит в том, чтобы сгенерировать последовательность строений робота, которая удовлетворяет следующим критериям:

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

numWaypoints = 5;
q0 = homeConfiguration(lbr);
qWaypoints = repmat(q0, numWaypoints, 1);

Создайте generalizedInverseKinematics решатель, который принимает следующие входы ограничений:

gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})

gik = 
  generalizedInverseKinematics with properties:

      NumConstraints: 5
    ConstraintInputs: {1x5 cell}
       RigidBodyTree: [1x1 rigidBodyTree]
     SolverAlgorithm: 'BFGSGradientProjection'
    SolverParameters: [1x1 struct]

Создание объектов ограничений

Создайте объекты ограничения, которые передаются в качестве входов решателю. Этот объект содержит параметры, необходимые для каждого ограничения. Измените эти параметры между вызовами решателя по мере необходимости.

Создайте ограничение Декартовых границ, которое требует, чтобы захват был по крайней мере на 5 см выше таблицы (отрицательное направление z). Все другие значения заданы как inf или -inf.

heightAboveTable = constraintCartesianBounds(gripper);
heightAboveTable.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.05, inf]

heightAboveTable = 
  constraintCartesianBounds with properties:

        EndEffector: 'iiwa_link_ee_kuka'
      ReferenceBody: ''
    TargetTransform: [4x4 double]
             Bounds: [3x2 double]
            Weights: [1 1 1]

Создайте ограничение на положение стакана относительно захвата с допуском 5 мм.

distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = gripper;
distanceFromCup.PositionTolerance = 0.005

distanceFromCup = 
  constraintPositionTarget with properties:

          EndEffector: 'cupFrame'
        ReferenceBody: 'iiwa_link_ee_kuka'
       TargetPosition: [0 0 0]
    PositionTolerance: 0.0050
              Weights: 1

Создайте ограничение прицеливания, которое требует оси Z iiwa_link_ee система координат должен быть приблизительно вертикальным, путем размещения цели далеко над роботом. The iiwa_link_ee система координат ориентирована так, что это ограничение выравнивает захват по оси чашки.

alignWithCup = constraintAiming('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]

alignWithCup = 
  constraintAiming with properties:

         EndEffector: 'iiwa_link_ee'
       ReferenceBody: ''
         TargetPoint: [0 0 100]
    AngularTolerance: 0
             Weights: 1

Создайте ограничение положения соединений. Установите Bounds свойство этого ограничения, основанное на предыдущем строении, чтобы ограничить изменение положений соединений.

limitJointChange = constraintJointBounds(lbr)

limitJointChange = 
  constraintJointBounds with properties:

     Bounds: [7x2 double]
    Weights: [1 1 1 1 1 1 1]

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

fixOrientation = constraintOrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1)

fixOrientation = 
  constraintOrientationTarget with properties:

             EndEffector: 'iiwa_link_ee_kuka'
           ReferenceBody: ''
       TargetOrientation: [1 0 0 0]
    OrientationTolerance: 0.0175
                 Weights: 1

Найдите строение, которая указывает на Кубок

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

intermediateDistance = 0.3;

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

limitJointChange.Weights = zeros(size(limitJointChange.Weights));
fixOrientation.Weights = 0;

Установите целевое положение чашки в системе координат захвата. Чашка должна лежать на оси Z захвата на заданном расстоянии.

distanceFromCup.TargetPosition = [0,0,intermediateDistance];

Решите для строения робота, который удовлетворяет вход ограничениям с помощью gik решатель. Необходимо задать все входные ограничения. Установите это строение как вторую путевую точку.

[qWaypoints(2,:),solutionInfo] = gik(q0, heightAboveTable, ...
                       distanceFromCup, alignWithCup, fixOrientation, ...
                       limitJointChange);

Найдите строения, которые перемещают Gripper в чашку вдоль прямой линии

Повторно включите ограничение положения соединения и ограничение ориентации.

limitJointChange.Weights = ones(size(limitJointChange.Weights));
fixOrientation.Weights = 1;

Отключите ограничение выравнивания по чашке, так как ограничение ориентации делает его избыточным.

alignWithCup.Weights = 0;

Установите ограничение ориентации, чтобы удерживать ориентацию на основе предыдущего строения (qWaypoints(2,:)). Получите преобразование от захвата к основе модели робота. Преобразуйте однородное преобразование в кватернион.

fixOrientation.TargetOrientation = ...
    tform2quat(getTransform(lbr,qWaypoints(2,:),gripper));

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

finalDistanceFromCup = 0.05;
distanceFromCupValues = linspace(intermediateDistance, finalDistanceFromCup, numWaypoints-1);

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

maxJointChange = deg2rad(10);

Вызовите решатель для каждой оставшейся путевой точки.

for k = 3:numWaypoints
    % Update the target position.
    distanceFromCup.TargetPosition(3) = distanceFromCupValues(k-1);
    % Restrict the joint positions to lie close to their previous values.
    limitJointChange.Bounds = [qWaypoints(k-1,:)' - maxJointChange, ...
                               qWaypoints(k-1,:)' + maxJointChange];
    % Solve for a configuration and add it to the waypoints array.
    [qWaypoints(k,:),solutionInfo] = gik(qWaypoints(k-1,:), ...
                                         heightAboveTable, ...
                                         distanceFromCup, alignWithCup, ...
                                         fixOrientation, limitJointChange);
end

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

Интерполируйте между путевыми точками, чтобы сгенерировать плавную траекторию. Использовать pchip чтобы избежать перерегулирований, которые могут нарушить пределы соединений робота.

framerate = 15;
r = rateControl(framerate);
tFinal = 10;
tWaypoints = [0,linspace(tFinal/2,tFinal,size(qWaypoints,1)-1)];
numFrames = tFinal*framerate;
qInterp = pchip(tWaypoints,qWaypoints',linspace(0,tFinal,numFrames))';

Вычислите положение захвата для каждой интерполированного строения.

gripperPosition = zeros(numFrames,3);
for k = 1:numFrames
    gripperPosition(k,:) = tform2trvec(getTransform(lbr,qInterp(k,:), ...
                                                    gripper));
end

Показать робота в его начальном строении вместе с таблицей и чашкой

figure;
show(lbr, qWaypoints(1,:), 'PreservePlot', false);
hold on
exampleHelperPlotCupAndTable(cupHeight, cupRadius, cupPosition);
p = plot3(gripperPosition(1,1), gripperPosition(1,2), gripperPosition(1,3));

Анимируйте манипулятор и постройте график положения захвата.

hold on
for k = 1:size(qInterp,1)
    show(lbr, qInterp(k,:), 'PreservePlot', false);
    p.XData(k) = gripperPosition(k,1);
    p.YData(k) = gripperPosition(k,2);
    p.ZData(k) = gripperPosition(k,3);
    waitfor(r);
end
hold off

Если необходимо сохранить сгенерированные строения в MAT-файле для дальнейшего использования, выполните следующее:

>> save('lbr_trajectory.mat', 'tWaypoints', 'qWaypoints');