Сгенерируйте Код, чтобы Запланировать и Выполнить использование Траекторий Без коллизий Манипулятор КИНОВОЙ Gen3

В этом примере показано, как сгенерировать код для того, чтобы ускорить планирование и выполнение траекторий робота без коллизий с обратной связью с помощью прогнозирующего управления модели (MPC). Для получения дополнительной информации о том, как использовать MPC для планирования движения манипуляторов робота, см. План в качестве примера и Выполните Траектории Без коллизий Используя Манипулятор КИНОВОЙ Gen3 (Robotics System Toolbox).

Модификации для генерации кода

Чтобы включить генерацию кода, адаптируйте код в Плане и Выполните Траектории Без коллизий Используя Манипулятор КИНОВОЙ Gen3 (Robotics System Toolbox), выполняющий шаги ниже.

  • Обеспечьте всему помощнику, Нелинейные функции MPC хранятся как отдельные программные файлы вместо Анонимных функций. Этот пример требует следующих файлов помощника.

- Пользовательская функция стоимости: nlmpcCostFunctionKINOVACodeGen.m

- Пользовательское неравенство constraints: nlmpcIneqConFunctionKINOVACodeGen.m

- Функция состояния: nlmpcModelKINOVACodeGen.m

- Выведите function: nlmpcOutputKINOVACodeGen.m

- Якобиан пользовательской стоимости function: nlmpcJacobianCostKINOVACodeGen.m

- Якобиан пользовательского неравенства constraints: nlmpcJacobianConstraintKINOVACodeGen.m

- Якобиан функции состояния: nlmpcJacobianModelKINOVACodeGen.m

- Якобиан выхода function: nlmpcJacobianOutputKINOVACodeGen.m

  • Используйте Parameters свойство nlmpcmoveopt объект (и, когда файл MEX, onlineData структура), чтобы передать контроллеру любой параметр, который может измениться во времени выполнения (то есть, на каждом временном шаге) или между последовательными запусками. Например, включить обновление препятствий позирует согласно новым показаниям датчика на каждом временном шаге, значениях текущих положений препятствия (posesNow) добавляются к params переменная ячейки. Точно так же обновить целевое положение робота онлайн, целевое положение (poseFinal) добавляется к Parameters список. Все параметры должны быть численными значениями. На каждом временном шаге, обновленном значении params затем копируется в Parameters свойство любого nlmpcmoveopt объект (если nlmpcmove используется), или в onlineData структура (если файл MEX используется).

  • Используйте persistent переменные, чтобы передать в переменных, которые не являются числовыми и внешне не изменяются во времени выполнения. Например, rigidBodyTree Объект (Robotics System Toolbox) robot и массив ячеек collisionMesh (Robotics System Toolbox) возражает world создаются и сохранены как персистентные переменные в первый раз, когда функция помощника выполняется с помощью следующего кода.

persistent robot world    
if isempty(robot)
    robot = loadrobot('kinovaGen3', 'DataFormat', 'column');  
    [world, ~] = helperCreateObstaclesKINOVA(posesNow);
end 
  • Гарантируйте, что все функции, используемые в файлах помощника, поддерживают генерацию кода. Для списка встроенных функций MATLAB, поддержанных для генерации кода, смотрите эту страницу. Чтобы протестировать, если пользовательская функция поддерживает генерацию кода, используйте coder.screener (Simulink). Например, попытайтесь запустить coder.screener('nlmpcIneqConFunctionKINOVA'). Этот пример требует того loadrobot (Robotics System Toolbox) и checkCollision (Robotics System Toolbox) генерация кода поддержки.

  • Используйте getCodeGenerationData и buildMEX сгенерировать код для nlmpc объект с заданными свойствами. Замените nlmpcmove в исходном примере со сгенерированным файлом MEX, чтобы вычислить действия оптимального управления.

Описание робота и положения

Загрузите модель дерева твердого тела (RBT) КИНОВОЙ Gen3.

robot = loadrobot('kinovaGen3', 'DataFormat', 'column');

Получите количество соединений.

numJoints = numel(homeConfiguration(robot));

Задайте систему координат робота, где исполнительный элемент конца присоединяется.

endEffector = "EndEffector_Link"; 

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

% Initial end-effector pose
taskInit = trvec2tform([[0.4 0 0.2]])*axang2tform([0 1 0 pi]);

% Compute current robot joint configuration using inverse kinematics
ik = inverseKinematics('RigidBodyTree', robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];
currentRobotJConfig = ik(endEffector, taskInit, weights, robot.homeConfiguration);
currentRobotJConfig = wrapToPi(currentRobotJConfig);

% Final (desired) end-effector pose
taskFinal = trvec2tform([0.35 0.55 0.35])*axang2tform([0 1 0 pi]);  
anglesFinal = rotm2eul(taskFinal(1:3,1:3),'XYZ');
poseFinal = [taskFinal(1:3,4);anglesFinal']; % 6x1 vector for final pose: [x, y, z, phi, theta, psi]

Сетки столкновения и препятствия

Чтобы проверять на и избежать столкновений во время управления, необходимо установить столкновение world в виде набора объектов столкновения. Этот пример использует collisionSphere (Robotics System Toolbox) возражает как препятствия, чтобы избежать. Чтобы запланировать использование, статическое вместо того, чтобы переместить объекты, установите isMovingObst к false.

isMovingObst = true;

Размеры препятствия и местоположения инициализируются в helperCreateMovingObstaclesKINOVACodeGen функция помощника. Чтобы добавить больше статических препятствий, добавьте объекты столкновения в world массив.

if isMovingObst == true
    helperCreateMovingObstaclesKINOVACodeGen
else
    posesNow = [0.4 0.4 0.25 ; 0.3 0.3 0.4];
end
[world, numObstacles] = helperCreateObstaclesKINOVACodeGen(posesNow);

Визуализируйте робота в начальной настройке. Необходимо видеть препятствия в среде также.

x0 = [currentRobotJConfig', zeros(1,numJoints)];
helperInitialVisualizerKINOVACodeGen;

Figure contains an axes. The axes contains 11 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Задайте расстояние безопасности далеко от препятствий. Это значение используется в функции ограничения неравенства нелинейного контроллера MPC.

safetyDistance = 0.01; 

Спроектируйте нелинейный прогнозирующий контроллер модели

Можно спроектировать нелинейный прогнозирующий контроллер модели, использующий helperDesignNLMPCobjKINOVACodeGen файл помощника, который создает nlmpc объект контроллера. Чтобы просмотреть файл, в командной строке MATLAB, вводят edit helperDesignNLMPCobjKINOVACodeGen.

helperDesignNLMPCobjKINOVACodeGen;

Планирование траектории с обратной связью

Симулируйте робота максимум для 50 шагов с правильными начальными условиями.

Инициализируйте параметры симуляции

maxIters = 50;
u0 = zeros(1,numJoints);
mv = u0';
time = 0;
goalReached = false;

Сгенерируйте Код для Объекта nlmpc

useMex = true;
buildMex = true;
if useMex
    [coredata, onlinedata] = getCodeGenerationData(nlobj,x0',u0',paras);
    if buildMex
        mexfcn = buildMEX(nlobj,'kinovaMex',coredata,onlinedata);
    end
end
Generating MEX function "kinovaMex" from nonlinear MPC to speed up simulation.
Code generation successful.

MEX function "kinovaMex" successfully generated.

Инициализируйте массив данных для управления

positions = zeros(numJoints,maxIters);
positions(:,1) = x0(1:numJoints)';

velocities = zeros(numJoints,maxIters);
velocities(:,1) = x0(numJoints+1:end)';

accelerations = zeros(numJoints,maxIters);
accelerations(:,1) = u0';

timestamp = zeros(1,maxIters);
timestamp(:,1) = time;

Сгенерируйте траекторию

Используйте сгенерированный файл MEX kinovaMex вместо nlmpcmove функция, чтобы ускорить генерацию траектории с обратной связью. Задайте генерацию траектории онлайновые опции с помощью аргумента onlineData.

Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий, когда они двигают цель. helperCheckGoalReachedKINOVACodeGen скрипт проверяет, достиг ли робот цели. helperUpdateMovingObstaclesKINOVACodeGen скрипт перемещает местоположения препятствия на основе временного шага.

options = nlmpcmoveopt;

for timestep=1:maxIters
    disp(['Calculating control at timestep ', num2str(timestep)]);
    % Optimize next trajectory point 
    if ~useMex
        options.Parameters = paras;
        [mv,options,info] = nlmpcmove(nlobj,x0,mv,[],[], options);
    else
        onlinedata.Parameters = paras; 
        [mv,onlinedata,info] = kinovaMex(x0',mv,onlinedata);
    end
    if info.ExitFlag < 0
        disp('Failed to compute a feasible trajectory. Aborting...')
        break;
    end
    % Update states and time for next iteration
    x0 = info.Xopt(2,:);
    time = time + nlobj.Ts;
    % Store trajectory points
    positions(:,timestep+1) = x0(1:numJoints)';
    velocities(:,timestep+1) = x0(numJoints+1:end)';
    accelerations(:,timestep+1) = info.MVopt(2,:)';
    timestamp(timestep+1) = time;
    % Check if goal is achieved 
    helperCheckGoalReachedKINOVACodeGen;
    if goalReached
        break;
    end
    % Update obstacle pose if it is moving
    if isMovingObst
        helperUpdateMovingObstaclesKINOVACodeGen;
    end
end
Calculating control at timestep 1
Calculating control at timestep 2
Calculating control at timestep 3
Calculating control at timestep 4
Calculating control at timestep 5
Calculating control at timestep 6
Calculating control at timestep 7
Calculating control at timestep 8
Target configuration reached.

Выполните запланированную траекторию Используя управление роботом низкого качества и симуляцию

Обрежьте векторы траектории на основе временных шагов, вычисленных из плана.

tFinal = timestep+1;
positions = positions(:,1:tFinal);
velocities = velocities(:,1:tFinal);
accelerations = accelerations(:,1:tFinal);
timestamp = timestamp(:,1:tFinal);

visTimeStep = 0.2;

Используйте jointSpaceMotionModel возразите, чтобы отследить траекторию с управлением вычисленного крутящего момента. helperTimeBasedStateInputsKINOVA функция генерирует производные входные параметры для ode15s Решатель ОДУ, чтобы симулировать перемещение робота вдоль вычисленной траектории.

motionModel = jointSpaceMotionModel('RigidBodyTree', robot);

% Control robot to target trajectory points in simulation using low-fidelity model
initState = [positions(:,1);velocities(:,1)];
targetStates = [positions;velocities;accelerations]';    
[t,robotStates] = ode15s(@(t,state) helperTimeBasedStateInputsKINOVA(motionModel, timestamp, targetStates, t, state), [timestamp(1):visTimeStep:timestamp(end)], initState);

Визуализируйте движение робота.

helperFinalVisualizerKINOVACodeGen;

Figure contains an axes. The axes contains 52 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

Как ожидалось робот успешно проходит запланированная траектория и избегает препятствий.

Смотрите также

|

Похожие темы

Для просмотра документации необходимо авторизоваться на сайте