Exponenta Banner
exponenta event banner

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

Открыть сценарий в реальном времени

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

См. также

Классы

Связанные темы

Документация по инструментам робототехнической системы

Поддержка