Exponenta Banner

Решите инверсную кинематику для четырёхзвенника

Скрипт Open Live Script

В этом примере показано, как решить инверсную кинематику для рычажного устройства закрытой цепи четырёхзвенника. Robotics System Toolbox™ непосредственно не поддерживает механизмы с обратной связью. Однако закрывающие цикл соединения могут быть аппроксимированы с помощью кинематических ограничений. В этом примере показано, как установить дерево твердого тела для четырёхзвенника, задайте кинематические ограничения и решите для желаемого положения исполнительного элемента конца.

Инициализируйте модель дерева твердого тела четырёхзвенника. 

robot = rigidBodyTree('Dataformat','column','MaxNumBodies',7);

Задайте имена тела, породите имена, объединенные имена, объединенные типы, и зафиксированный преобразовывает в массивы ячеек. Фиксированные преобразования задают геометрию четырёхзвенника. Рычажное устройство вращается в xz-плоскости. Смещение -0.1 используется в оси Y на 'b4' тело, чтобы изолировать движение перекрывающихся соединений для 'b3' и 'b4'.

bodyNames = {'b1','b2','b3','b4','b5','b6'};
parentNames = {'base','b1','b2','base','b4','b5'};
jointNames = {'j1','j2','j3','j4','j5','j6'};
jointTypes = {'revolute','revolute','fixed','revolute','revolute','fixed'};
fixedTforms = {eye(4), ...
                trvec2tform([0 0 0.5]), ...
                trvec2tform([0.8 0 0]), ...
                trvec2tform([0.0 -0.1 0]), ...
                trvec2tform([0.8 0 0]), ...
                trvec2tform([0 0 0.5])};

Используйте for цикл, чтобы собрать четырёхзвенник:

  • Создайте твердое тело и задайте объединенный тип.

  • Задайте JointAxis свойство для любых нефиксированных соединений.

  • Задайте фиксированное преобразование.

  • Добавьте тело в дерево твердого тела.

for k = 1:6

    b = rigidBody(bodyNames{k});
    b.Joint = rigidBodyJoint(jointNames{k},jointTypes{k});
    
    if ~strcmp(jointTypes{k},'fixed')
        b.Joint.JointAxis = [0 1 0];
    end
    
    b.Joint.setFixedTransform(fixedTforms{k});
    
    addBody(robot,b,parentNames{k});
end

Добавьте итоговое тело, чтобы функционировать как исполнительный элемент конца (указатель) для четырёхзвенника.

bn = 'handle';
b = rigidBody(bn);
setFixedTransform(b.Joint,trvec2tform([0 -0.15 0]));
addBody(robot,b,'b6');

Задайте кинематические ограничения для GeneralizedInverseKinematics объект:

  • Ограничение положения 1: источники 'b3' система координат тела и 'b6' система координат тела должна всегда перекрываться. Это сохраняет указатель в соответствии с аппроксимированным механизмом с обратной связью. Используйте -0.1 возместите для y-координаты.

  • Ограничение положения 2: исполнительный элемент конца должен предназначаться для желаемого положения.

  • Объединенные предельные границы: Удовлетворите объединенным пределам в модели дерева твердого тела.

gik = generalizedInverseKinematics('RigidBodyTree',robot);
gik.ConstraintInputs = {'position',...  % Position constraint for closed-loop mechanism
                        'position',...  % Position constraint for end-effector 
                        'joint'};       % Joint limits
gik.SolverParameters.AllowRandomRestart = false;

% Position constraint 1
positionTarget1 = constraintPositionTarget('b6','ReferenceBody','b3');
positionTarget1.TargetPosition = [0 -0.1 0];
positionTarget1.Weights = 50;
positionTarget1.PositionTolerance = 1e-6;

% Joint limit bounds
jointLimBounds = constraintJointBounds(gik.RigidBodyTree);
jointLimBounds.Weights = ones(1,size(gik.RigidBodyTree.homeConfiguration,1))*10;

% Position constraint 2
desiredEEPosition = [0.9 -0.1 0.9]'; % Position is relative to base.
positionTarget2 = constraintPositionTarget('handle');
positionTarget2.TargetPosition = desiredEEPosition; 
positionTarget2.PositionTolerance = 1e-6;
positionTarget2.Weights = 1;

Вычислите кинематическое решение с помощью gik объект. Задайте исходное предположение и различные кинематические ограничения в соответствующем порядке.

iniGuess = homeConfiguration(robot);
[q, solutionInfo] = gik(iniGuess,positionTarget1,positionTarget2,jointLimBounds);

Исследуйте результаты в solutionInfo. Покажите кинематическое решение по сравнению с домашней настройкой. Графики показывают в xz-плоскости.

loopClosingViolation = solutionInfo.ConstraintViolations(1).Violation;
jointBndViolation = solutionInfo.ConstraintViolations(2).Violation;
eePositionViolation = solutionInfo.ConstraintViolations(3).Violation;

subplot(1,2,1)
show(robot,homeConfiguration(robot));
title('Home Configuration')
view([0 -1 0]);
subplot(1,2,2)
show(robot,q);
title('GIK Solution')
view([0 -1 0]);

Figure contains 2 axes objects. Axes object 1 with title Home Configuration contains 15 objects of type patch, line. These objects represent base, b1, b2, b3, b4, b5, b6, handle. Axes object 2 with title GIK Solution contains 15 objects of type patch, line. These objects represent base, b1, b2, b3, b4, b5, b6, handle.

Смотрите также

Классы

Похожие темы

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

Поддержка