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

В этом примере используется модель 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 чтобы рама была приблизительно вертикальной, поместив цель далеко над роботом. 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);

Поиск конфигураций, перемещающих захват в чашу вдоль прямой линии

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

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

Отключите ограничение align-with-cup, так как ограничение ориентации делает его избыточным.

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');