Изменения для генерации кода

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

- Функция пользовательских затрат: nlmpcCostFunctionKINOVACodeGen.m

- Пользовательские ограничения неравенства: nlmpcIneqConFunctionKINOVACodeGen.m

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

- Выходная функция: nlmpcOutputKINOVACodeGen.m

- Якобиан пользовательской функции затрат: nlmpcJacobianCostKINOVACodeGen.m

- якобиан пользовательских ограничений неравенства: nlmpcJacobianConstraintKINOVACodeGen.m

- якобиан государственной функции: nlmpcJacobianModelKINOVACodeGen.m

- Якобиан выходной функции: nlmpcJacobianOutputKINOVACodeGen.m

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

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

Загрузите модель KINOVA Gen3 древовидного твердого тела (RBT).

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.

Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий, когда они движутся к цели. The helperCheckGoalReachedKINOVACodeGen скрипт проверяет, достиг ли робот цели. The 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 объект для отслеживания траектории с вычисленным крутящим моментом. The 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;

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