Введение

В этом примере показано, как вычислить обратную кинематику для простого манипулятора 2D с помощью inverseKinematics класс. Робот-манипулятор представляет собой простой плоский манипулятор 2 степени свободы с поворотными соединениями, который создается путем сборки жестких тел в rigidBodyTree объект. Круговая траектория создается в плоскости 2-D и задается как точки для решения обратной кинематики. Решатель вычисляет требуемые положения соединения для достижения этой траектории. Наконец, робот анимируется, чтобы показать конфигурации робота, которые достигают круговой траектории.

Построение робота

Создать rigidBodyTree объект и жесткие тела с сопряженными с ними соединениями. Задайте геометрические свойства каждого жесткого тела и добавьте его к роботу.

Начните с пустой модели дерева жесткого тела.

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',3);

Укажите длину руки робота.

L1 = 0.3;
L2 = 0.3;

Добавить 'link1' тело с 'joint1' сочленение.

body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');

Добавить 'link2' тело с 'joint2' сочленение.

body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([L1,0,0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link1');

Добавить 'tool' конечный эффектор с 'fix1' неподвижное соединение.

body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L2, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link2');

Отображение сведений о роботе для проверки входных свойств. Робот должен иметь два неразъемных соединения для жестких тел и неподвижное тело для концевого эффектора.

showdetails(robot)

--------------------
Robot: (3 bodies)

 Idx    Body Name    Joint Name    Joint Type    Parent Name(Idx)   Children Name(s)
 ---    ---------    ----------    ----------    ----------------   ----------------
   1        link1        joint1      revolute             base(0)   link2(2)  
   2        link2        joint2      revolute            link1(1)   tool(3)  
   3         tool          fix1         fixed            link2(2)   
--------------------

Определение траектории

Определите круг, который будет отслеживаться в течение 10 секунд. Эта окружность находится в плоскости xy с радиусом 0,15.

t = (0:0.2:10)'; % Time
count = length(t);
center = [0.3 0.1 0];
radius = 0.15;
theta = t*(2*pi/t(end));
points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

Решение по обратной кинематике

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

Предварительное распределение конфигурационных решений в виде матрицы qs.

q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

Создайте решатель обратной кинематики. Поскольку декартовы точки xy являются единственными важными факторами положения конечного эффектора для этого рабочего процесса, укажите ненулевой вес для четвертого и пятого элементов weight вектор. Все остальные элементы равны нулю.

ik = inverseKinematics('RigidBodyTree', robot);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

Замкните траекторию точек, чтобы проследить окружность. Позвоните в ik объект для каждой точки, чтобы сформировать конфигурацию соединения, которая достигает конечного положения эффектора. Сохраните конфигурации, которые будут использоваться позже.

qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

Анимация решения

Постройте график робота для каждого кадра решения с использованием конкретной конфигурации робота. Также постройте график нужной траектории.

Показать робота в первой конфигурации траектории. Настройте график, чтобы показать 2-D плоскость, на которой нарисована окружность. Постройте график нужной траектории.

figure
show(robot,qs(1,:)');
view(2)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.1 0.7 -0.3 0.5])

Настройка rateControl объект для отображения траектории робота с фиксированной скоростью 15 кадров в секунду. Отображение робота в каждой конфигурации из обратного кинематического решателя. Следите за тем, как рука отслеживает показанную круговую траекторию.

framesPerSecond = 15;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:)','PreservePlot',false);
    drawnow
    waitfor(r);
end