2D трассировка пути с инверсной кинематикой

Введение

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

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

Создайте 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

Анимируйте решение

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

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

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

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