Ускорьте алгоритмы робототехники с генерацией кода

Можно сгенерировать код для, выбирают алгоритмы Robotics System Toolbox™, чтобы ускорить их выполнение. Настройте алгоритм, который поддерживает генерацию кода как отдельную функцию, которую можно вставить в рабочий процесс. Чтобы использовать генерацию кода, у вас должна быть лицензия MATLAB^® Coder™. Для сводных данных поддержки генерации кода в Robotics System Toolbox смотрите Генерацию кода.

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

Создайте отдельную функцию для алгоритма

Создайте отдельную функцию, vfhCodeGen, это запускает алгоритм инверсной кинематики. Создайте inverseKinematics возразите и создайте rigidBodyTree модель в функции. Задайте %#codegen в функции, чтобы идентифицировать его как функцию для генерации кода.

function qConfig = ikCodegen(endEffectorName,tform,weights,initialGuess)
	%#codegen
    
    robot = rigidBodyTree('MaxNumBodies',3,'DataFormat','row');
    body1 = rigidBody('body1');
    body1.Joint = rigidBodyJoint('jnt1','revolute');

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

    body3 = rigidBody('tool');
    jnt3 = rigidBodyJoint('jnt3','revolute');
    setFixedTransform(jnt3,trvec2tform([1 0 0]))
    body3.Joint = jnt3;
    
    addBody(robot,body1,'base')
    addBody(robot,body2,'body1')
    addBody(robot,body3,'body2')


	ik = inverseKinematics('RigidBodyTree',robot);
	
	[qConfig,~] = ik(endEffectorName,tform,weights,initialGuess); 
end

Сохраните функцию в своей текущей папке.

Выполните генерацию кода для алгоритма

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

Задайте демонстрационные значения для входных параметров.

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

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

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

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

Проверяйте производительность сгенерированного кода

Сравните синхронизацию сгенерированной MEX-функции к синхронизации вашей исходной функции при помощи timeit.

time = timeit(@() ikCodegen(rbt,endEffectorName,tform,weights,initialGuess))
mexTime = timeit(@() ikCodegen_mex(rbt,endEffectorName,tform,weights,initialGuess))

time =

    0.0425


mexTime =

    0.0011

MEX-функция работает в 30 раз быстрее в этом примере. Результаты могут варьироваться по вашей системе.

Замените функцию алгоритма на MEX-функцию

Откройте основную функцию для выполнения вашего рабочего процесса робототехники. Замените ik вызов объектов с MEX-функцией, что вы создали генерацию кода использования. В данном примере используйте простой 2D пример трассировки пути.

Откройте 2D Трассировку Пути С примером Инверсной кинематики.

openExample('robotics/TwoDInverseKinematicsExampleExample')

Измените пример кода, чтобы использовать новый ikCodeGen_mex функция. Код, который следует, является копией примера с модификациями к использованию новой MEX-функции. Определение модели робота сделано в функции, так пропустите раздел Construct the Robot.

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

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-функцию в рабочем процессе.

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