Документация

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

Этот пример использует модель LBR KUKA iiwa, 7 манипуляторов робота степени свободы. importrobot генерирует модель robotics.RigidBodyTree из описания, сохраненного в файле Объединенного формата описания робота (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 = robotics.RigidBody('cupFrame');
setFixedTransform(body.Joint, trvec2tform(cupPosition))
addBody(lbr, body, lbr.BaseName);

Задайте проблему планирования

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

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

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

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

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

gik = 
  robotics.GeneralizedInverseKinematics with properties:

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

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

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

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

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

heightAboveTable = 
  CartesianBounds with properties:

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

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

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

distanceFromCup = 
  PositionTarget 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 = robotics.AimingConstraint('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]

alignWithCup = 
  AimingConstraint with properties:

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

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

limitJointChange = robotics.JointPositionBounds(lbr)

limitJointChange = 
  JointPositionBounds with properties:

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

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

fixOrientation = robotics.OrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1)

fixOrientation = 
  OrientationTarget 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. Необходимо задать все входные ограничения. Установите ту настройку как второй waypoint.

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

Найдите настройки, которые перемещают механизм захвата в Кубок вдоль прямой линии

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

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

Отключите выровнять с чашкой ограничение, когда ограничение ориентации сокращает его.

alignWithCup.Weights = 0;

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

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

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

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

Задайте максимальное позволенное изменение в объединенных положениях между каждым waypoint.

maxJointChange = deg2rad(10);

Вызовите решатель для каждого остающегося waypoint.

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

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

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

framerate = 15;
r = robotics.Rate(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');