В этом примере показано, как выполнить генерацию кода для вычисления обратной кинематики (IK) с помощью роботов из библиотеки роботов. Для этого примера можно использовать inverseKinematics объект с включенным rigidBodyTree модель робота с использованием loadrobot для решения для конфигураций роботов, которые достигают желаемого конечного положения эффектора.
Круговая траектория создается в плоскости 2-D и задается в качестве точек для генерируемого решателя обратной кинематики 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 в текущей папке.
Перед созданием кода проверьте алгоритм 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) или приложение Кодер MATLAB (Кодер MATLAB) для создания кода. В этом примере создайте файл MEX путем вызова codegen в командной строке MATLAB. Укажите примеры входных аргументов для каждого ввода в функцию с помощью -args входной аргумент.
Позвоните в codegen и укажите входные аргументы в массиве ячеек. Эта функция создает отдельный ikCodegen_mex функция для использования. Можно также создать код C с помощью входного аргумента опций. Этот шаг может занять некоторое время.
codegen ikCodegen -args {endEffectorName,targetPose,weights,initialGuess}
Code generation successful.
Вызовите версию 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 для вычисления решения «Обратная кинематика» для достижения траектории.
Создайте круговую траекторию.
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
