Введение

В этом примере показано, как вычислить обратную кинематику для простого манипулятора 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 Декартовы точки являются единственными важными факторами положения end-effector для этого рабочего процесса, задайте ненулевой вес для четвертого и пятого элементов weight вектор. Все остальные элементы равны нулю.

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

Проследите контур траектории точек, чтобы проследить окружность. Вызовите ik объект для каждой точки, чтобы сгенерировать строение соединения, которая достигает положения end-effector. Сохраните строения, которые будут использоваться позже.

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