Документация

Укажите примерные значения для входных аргументов.

endEffectorName = 'tool';       
tform = trvec2tform([0.7 -0.7 0]);
weights = [0.25 0.25 0.25 1 1 1];
initialGuess = [0 0 0];

Позвоните в codegen и укажите входные аргументы в массиве ячеек. Эта функция создает отдельный ikCodegen_mex функция для использования. Можно также создать код C с помощью options входной аргумент.

codegen ikCodegen -args {endEffectorName,tform,weights,initialGuess}

Если входные данные могут исходить из значений переменной длины, укажите канонический тип входных данных с помощью coder.typeof (Конструктор фиксированных точек) с помощью codegen (Кодер MATLAB).

Откройте 2-е Отслеживание Пути с Обратным примером Кинематики.

openExample('robotics/TwoDInverseKinematicsExampleExample')

Изменение кода примера для использования нового ikCodegen_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];
ndof = length(q0);
qs = zeros(count, ndof);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

Замкните траекторию точек, чтобы проследить окружность. Замените ik вызов объекта с помощью 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

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

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

robot = rigidBodyTree('MaxNumBodies',15,'DataFormat','row');
body1 = rigidBody('body1');
body1.Joint = rigidBodyJoint('jnt1','revolute');

body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
setFixedTransform(jnt2,trvec2tform([0.3 0 0]))
body2.Joint = jnt2;

body3 = rigidBody('tool');
jnt3 = rigidBodyJoint('jnt3','revolute');
setFixedTransform(jnt3,trvec2tform([0.3 0 0]))
body3.Joint = jnt3;

addBody(robot,body1,'base')
addBody(robot,body2,'body1')
addBody(robot,body3,'body2')

% Show first solution and set view.
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])

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

В этом примере показано, как можно создать код для определенных алгоритмов или функций, чтобы повысить их скорость и просто заменить их созданной функцией MEX в рабочем процессе.