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

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

Инициализируйте модель дерева твердого тела связи с четырьмя панелями.

robot = robotics.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 = robotics.RigidBody(bodyNames{k});
    b.Joint = robotics.Joint(jointNames{k},jointTypes{k});
    
    if ~strcmp(jointTypes{k},'fixed')
        b.Joint.JointAxis = [0 1 0];
    end
    
    b.Joint.setFixedTransform(fixedTforms{k});
    
    robot.addBody(b,parentNames{k});
end

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

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

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

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

gik = robotics.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 = robotics.PositionTarget('b6','ReferenceBody','b3');
positionTarget1.TargetPosition = [0 -0.1 0];
positionTarget1.Weights = 50;
positionTarget1.PositionTolerance = 1e-6;

% Joint limit bounds
jointLimBounds = robotics.JointPositionBounds(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 = robotics.PositionTarget('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]);

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

Классы

robotics.GeneralizedInverseKinematics | robotics.InverseKinematics | robotics.JointPositionBounds | robotics.PoseTarget | robotics.PositionTarget | robotics.RigidBodyTree

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