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

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

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

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

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

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

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

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

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

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

persistent robot world    
if isempty(robot)
    robot = loadrobot('kinovaGen3', 'DataFormat', 'column');  
    [world, ~] = helperCreateObstaclesKINOVA(posesNow);
end 

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

Загрузите модель дерева твердого тела (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;

Задайте расстояние безопасности далеко от препятствий. Это значение используется в функции ограничения неравенства нелинейного контроллера 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;

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