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

Создайте функцию, 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);

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

Можно использовать любого 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);

Вычислите Инверсную кинематику с 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