В этом примере показано, как выполнить генерацию кода для вычисления обратной кинематики (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 перед генерацией кода.
Загрузите предопределенную модель робота 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 решателя 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. Задайте веса для преобразования 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
