В этом примере показано, как выполнить генерацию кода, чтобы вычислить Инверсную кинематику (IK) с помощью роботов от библиотеки робота. В данном примере можно использовать inverseKinematics объект с включенным rigidBodyTree модель робота использование loadrobot решить для настроек робота, которые достигают желаемого положения исполнительного элемента конца.
Круговая траектория создана в 2D плоскости и дана как точки к сгенерированному решателю инверсной кинематики 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 Coder) функция или MATLAB Coder (MATLAB Coder) приложение, чтобы сгенерировать код. В данном примере сгенерируйте файл MEX путем вызова codegen в командной строке MATLAB. Задайте демонстрационные входные параметры для каждого входа к функции с помощью -args входной параметр.
Вызовите codegen функционируйте и задайте входные параметры в массиве ячеек. Эта функция создает отдельный ikCodegen_mex функционируйте, чтобы использовать. Можно также произвести код С при помощи входного параметра опций. Этот шаг может занять время.
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.3084 2.2000 -1.2999 1.0092 2.0277 -2.0500 -0.0872
Визуализируйте робота с настройкой робота, вычисленной с помощью версии 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.3 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
