Создайте робота Delta

Обычно, роботы дельты содержат кинематические цепи с обратной связью. rigidBodyTree объект не поддерживает цепи с обратной связью. Чтобы избежать этого, робот моделируется как дерево руками робота дельты, остающегося несвязанным. Вызовите функцию помощника, которая создает модель робота и выводит rigidBodyTree object.

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

Робот является справедливо сложным, таким образом, функция помощника используется, чтобы создать объект rigidBodyTree.

robot = exampleHelperDeltaRobot;
show(robot);

Как показано робот состоит из трех рук, но они все еще должны быть соединены, чтобы совпадать с классической настройкой робота дельты.

Создайте обратные кинематические ограничения

Создайте generalizedInverseKinematics объект, и задает модель робота. Ограничьте максимальное количество взаимодействий на основе эффективности.

gik1 = generalizedInverseKinematics('RigidBodyTree', robot);
gik1.SolverParameters.MaxIterations = 20;

Создайте interactiveRigidBodyTree объект визуализировать модель робота и обеспечить интерактивные маркеры для движущихся контактирующих тел. Эта интерактивность помогает проверить ваши кинематические ограничения. Задайте gik1 решатель с помощью пар "имя-значение". Задайте вектор веса положения, который только фокусируется на xyz-позиционном а не ориентации.

viztree = interactiveRigidBodyTree(robot, 'IKSolver', gik1, 'SolverPoseWeights', [0 1]);

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

Сохраните текущую систему координат.

ax = gca;

Добавьте ограничения в решатель GIK, чтобы гарантировать, что руки соединяются. Присоедините эти две руки без исполнительного элемента конца к 6-му телу первичной руки, которая включает исполнительный элемент конца.

% Ensure that the body 6 of arm 2 maintains a pose relative to body 6 of arm 1
poseTgt1 = constraintPoseTarget('arm2_body6');
poseTgt1.ReferenceBody = 'arm1_body6';
poseTgt1.TargetTransform = trvec2tform([-sqrt(3)*0.5*0.2, 0.5*0.2, 0]) * eul2tform([2*pi/3, 0, 0]);

% Ensure that the body 6 of arm 3 maintains a pose relative to body 6 of arm 1
poseTgt2 = constraintPoseTarget('arm3_body6');
poseTgt2.ReferenceBody = 'arm1_body6';
poseTgt2.TargetTransform = trvec2tform([-sqrt(3)*0.5*0.2, -0.5*0.2, 0]) * eul2tform([-2*pi/3, 0, 0]);

Чтобы применить эти ограничения к роботу, вызовите addConstraint к vizTree объект.

addConstraint(viztree,poseTgt1);
addConstraint(viztree,poseTgt2);

Теперь, когда исполнительный элемент конца перемещен, ограничения уважают, и руки остаются на связи.

Решите обобщенную инверсную кинематику программно

Интерактивная визуализация полезна для проверки ограничений решателя, но для прямого программируемого использования, создайте отдельный решатель GIK, который может быть назван. Этот решатель может быть скопирован с IKSolver свойство interactiveRigidBodyTree объект, или созданный независимо.

gik2 = generalizedInverseKinematics('RigidBodyTree', robot);
gik2.SolverParameters.MaxIterations = 20;

Для решателя GIK дополнительное ограничение требуется, чтобы задавать положение исполнительного элемента конца, которым обычно управляет интерактивный маркер. Обновите TargetTransform решить для различных желаемых положений исполнительного элемента конца.

poseTgt3 = constraintPoseTarget('endEffector');
poseTgt3.ReferenceBody = 'base';
poseTgt3.TargetTransform = trvec2tform([0, 0, -1]);

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

gik2.ConstraintInputs = {'pose','pose', 'pose'};

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

% Provide an initial guess for the solver
q0 = homeConfiguration(robot);

% Solve for a the target pose given to poseTgt3
[q, solutionInfo] = gik2(q0, poseTgt1, poseTgt2, poseTgt3);

% Visualize the results
figure;
show(robot, q);