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

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

Загрузите предопределенную модель робота Gen3 KINOVA ® как r igidBodyTree объект. Установите формат данных равным "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)   
--------------------

Задайте имя end-effector, веса для преобразования end-effector и начальные положения соединений.

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

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

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. Задайте веса для преобразования end-effector и имени end-effector.

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 функция для вычисления решения для каждой точки, чтобы сгенерировать строение соединения, которая достигает положения 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 = 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