Сгенерируйте код для расчета инверсной кинематики Используя робота от библиотеки робота

В этом примере показано, как выполнить генерацию кода, чтобы вычислить Инверсную кинематику (IK) с помощью роботов от библиотеки робота. В данном примере можно использовать inverseKinematics объект с включенным rigidBodyTree модель робота использование loadrobot решить для настроек робота, которые достигают желаемого положения исполнительного элемента конца.

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

Напишите алгоритм, чтобы решить инверсную кинематику

Создайте функцию, ikCodegen, это запускает алгоритм инверсной кинематики для модели робота KINOVA® Gen3, созданной с помощью loadrobot.

function qConfig = ikCodegen(endEffectorName,tform,weights,initialGuess)
    %#codegen
    robot = loadrobot("kinovaGen3","DataFormat","row");
    ik = inverseKinematics('RigidBodyTree',robot);
    [qConfig,~] = ik(endEffectorName,tform,weights,initialGuess); 
end

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

Проверьте алгоритм инверсной кинематики в MATLAB

Проверьте алгоритм IK в MATLAB прежде, чем сгенерировать код.

Загрузите предопределенную модель робота KINOVA® Gen3 как rigidBodyTree объект. Установите формат данных на "row".

robot = loadrobot("kinovaGen3","DataFormat","row");

Покажите детали робота.

showdetails(robot)
--------------------
Robot: (8 bodies)

 Idx               Body Name         Joint Name         Joint Type               Parent Name(Idx)   Children Name(s)
 ---               ---------         ----------         ----------               ----------------   ----------------
   1           Shoulder_Link          Actuator1           revolute                   base_link(0)   HalfArm1_Link(2)  
   2           HalfArm1_Link          Actuator2           revolute               Shoulder_Link(1)   HalfArm2_Link(3)  
   3           HalfArm2_Link          Actuator3           revolute               HalfArm1_Link(2)   ForeArm_Link(4)  
   4            ForeArm_Link          Actuator4           revolute               HalfArm2_Link(3)   Wrist1_Link(5)  
   5             Wrist1_Link          Actuator5           revolute                ForeArm_Link(4)   Wrist2_Link(6)  
   6             Wrist2_Link          Actuator6           revolute                 Wrist1_Link(5)   Bracelet_Link(7)  
   7           Bracelet_Link          Actuator7           revolute                 Wrist2_Link(6)   EndEffector_Link(8)  
   8        EndEffector_Link        Endeffector              fixed               Bracelet_Link(7)   
--------------------

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

endEffectorName = 'EndEffector_Link';
weights = [0.25 0.25 0.25 1 1 1];
initialGuess = [0 0 0 0 0 0 0];

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

targetPose = trvec2tform([0.35 -0.35 0]);
qConfig = ikCodegen(endEffectorName,targetPose,weights,initialGuess)
qConfig = 1×7

    1.3085    2.2000   -1.3011    1.0072   -1.1144    2.0500   -3.2313

Визуализируйте робота с вычисленным решением для настройки робота.

figure;
show(robot,qConfig);
hold all
plotTransforms(tform2trvec(targetPose),tform2quat(targetPose),"FrameSize",0.5);

Figure contains an axes. The axes contains 29 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Сгенерируйте код для алгоритма инверсной кинематики

Можно использовать любого codegen (MATLAB Coder) функция или MATLAB Coder (MATLAB Coder) приложение, чтобы сгенерировать код. В данном примере сгенерируйте файл MEX путем вызова codegen в командной строке MATLAB. Задайте демонстрационные входные параметры для каждого входа к функции с помощью -args входной параметр.

Вызовите codegen функционируйте и задайте входные параметры в массиве ячеек. Эта функция создает отдельный ikCodegen_mex функционируйте, чтобы использовать. Можно также произвести код С при помощи входного параметра опций. Этот шаг может занять время.

codegen ikCodegen -args {endEffectorName,targetPose,weights,initialGuess}
Code generation successful.

Проверьте результаты Используя сгенерированную MEX-функцию

Вызовите версию MEX решателя IK для заданного преобразования.

targetPose = trvec2tform([0.35 -0.35 0]);
qConfig = ikCodegen_mex(endEffectorName,targetPose,weights,initialGuess)
qConfig = 1×7

    1.3085    2.2000   -1.3011    1.0072   -1.1144    2.0500   -3.2313

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

figure;
show(robot,qConfig);
hold all
plotTransforms(tform2trvec(targetPose),tform2quat(targetPose),"FrameSize",0.5);

Figure contains an axes. The axes contains 29 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Вычислите Инверсную кинематику с MEX-функцией

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

Задайте траекторию

Создайте круговую траекторию.

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))];

Решение для инверсной кинематики

Предварительно выделите решения для настройки как матричный qs. Задайте веса для преобразования исполнительного элемента конца и имени исполнительного элемента конца.

q0 = [0 0 0 0 0 0 0];
ndof = length(q0);
qs = zeros(count,ndof);
weights = [0 0 0 1 1 1];
endEffector = 'EndEffector_Link';

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

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 = ikCodegen_mex(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

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

Если вы генерируете все решения, анимируете результаты. Необходимо воссоздать робота, потому что он был первоначально задан в функции. Выполните итерации через все решения. Установите "FastUpdate" опция show метод к true получить плавную анимацию.

robot = loadrobot("kinovaGen3","DataFormat","row");
% Show first solution and set view.
figure
show(robot,qs(1,:));
view(3)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.1 0.7 -0.3 0.5])

% Iterate through the solutions
framesPerSecond = 15;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:),'PreservePlot',false,"FastUpdate",true);
    drawnow
    waitfor(r);
end

Figure contains an axes. The axes contains 26 objects of type patch, line. This object represents base_link.

Для просмотра документации необходимо авторизоваться на сайте