Этот пример показывает, как вычислить инверсную кинематику для простого 2D манипулятора с помощью класса robotics.InverseKinematics
. Робот манипулятора является простыми 2 степенями свободы плоский манипулятор с шарнирными соединениями, который создается путем сборки твердых тел в объект robotics.RigidBodyTree
. Круговая траектория создана в 2D плоскости и дана как точки к решателю инверсной кинематики. Решатель вычисляет необходимые объединенные положения, чтобы достигнуть этой траектории. Наконец, робот анимирован, чтобы показать настройки робота, которые достигают круговой траектории.
Создайте объект RigidBodyTree
и твердые тела с их связанными соединениями. Задайте геометрические свойства каждого твердого тела и добавьте его в робота.
Запустите с пустой модели дерева твердого тела.
robot = robotics.RigidBodyTree('DataFormat','column','MaxNumBodies',3);
Задайте длины руки для манипулятора.
L1 = 0.3; L2 = 0.3;
Добавьте тело 'link1'
с соединением 'joint1'
.
body = robotics.RigidBody('link1'); joint = robotics.Joint('joint1', 'revolute'); setFixedTransform(joint,trvec2tform([0 0 0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'base');
Добавьте тело 'link2'
с соединением 'joint2'
.
body = robotics.RigidBody('link2'); joint = robotics.Joint('joint2','revolute'); setFixedTransform(joint, trvec2tform([L1,0,0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'link1');
Добавьте исполнительный элемент конца 'tool'
с 'fix1'
зафиксированное соединение.
body = robotics.RigidBody('tool'); joint = robotics.Joint('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 = robotics.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])
Настройте объект robotics.Rate
отобразить траекторию робота по фиксированной процентной ставке 15 кадров в секунду. Покажите робота в каждой настройке от обратного кинематического решателя. См., когда рука прослеживает круговую показанную траекторию.
framesPerSecond = 15; r = robotics.Rate(framesPerSecond); for i = 1:count show(robot,qs(i,:)','PreservePlot',false); drawnow waitfor(r); end
InverseKinematics
| Joint
| RigidBody
| RigidBodyTree