Настройка Gazebo с моделью робота и плагином

Чтобы настроить мир Gazebo, установите необходимые плагины и протестируйте соединение с MATLAB и simulink, смотрите раздел Set Up Gazebo with Robot Model и Plugin в Configure Gazebo и Simulink for Co-simulation примера Робот.

Открытый мир в Gazebo

Откройте мир, запустив эти команды в терминале машины Gazebo:

cd /home/user/src/GazeboPlugin/export
export SVGA_VGPU10=0
gazebo /home/user/worlds/Ur10BasicWithPlugin.world --verbose

Gazebo показывает робота и любые другие объекты в мире. Если симулятор Gazebo не откроется, возможно, потребуется переустановить плагин. См. раздел Установка плагина Gazebo вручную в разделе «Совместное моделирование между Simulink и Gazebo».

Подключение к Gazebo

Затем инициализируйте соединение Gazebo с MATLAB и Simulink. Укажите IP-адрес и номер порта 14581, который является портом по умолчанию для плагина Gazebo.

ipGazebo = '192.168.116.162'; % Replace this with the IP of the Gazebo machine
gzinit(ipGazebo,14581);

Загрузка модели робота

Эта модель использует универсальный робота- UR10, созданного с помощью loadrobot. Модель Gazebo и модели робота совпадают, так как они из одного хранилища источников. Для получения дополнительной информации смотрите Настройте Gazebo и Simulink для Ко-симуляции робота.

robot = loadrobot('universalUR10','Gravity',[0 0 -9.81],'DataFormat','column');
showdetails(robot)

--------------------
Robot: (10 bodies)

 Idx             Body Name                            Joint Name                            Joint Type             Parent Name(Idx)   Children Name(s)
 ---             ---------                            ----------                            ----------             ----------------   ----------------
   1             base_link                           world_joint                                 fixed                     world(0)   base(2)  shoulder_link(3)  
   2                  base            base_link-base_fixed_joint                                 fixed                 base_link(1)   
   3         shoulder_link                    shoulder_pan_joint                              revolute                 base_link(1)   upper_arm_link(4)  
   4        upper_arm_link                   shoulder_lift_joint                              revolute             shoulder_link(3)   forearm_link(5)  
   5          forearm_link                           elbow_joint                              revolute            upper_arm_link(4)   wrist_1_link(6)  
   6          wrist_1_link                         wrist_1_joint                              revolute              forearm_link(5)   wrist_2_link(7)  
   7          wrist_2_link                         wrist_2_joint                              revolute              wrist_1_link(6)   wrist_3_link(8)  
   8          wrist_3_link                         wrist_3_joint                              revolute              wrist_2_link(7)   ee_link(9)  tool0(10)  
   9               ee_link                        ee_fixed_joint                                 fixed              wrist_3_link(8)   
  10                 tool0        wrist_3_link-tool0_fixed_joint                                 fixed              wrist_3_link(8)   
--------------------

Установите начальное строение робота.

q0 = [0 -70 140 0 0 0]' * pi/180; 

Загрузка модели Simulink

The gazeboCosimControl модель управляет положением конечного эффектора манипулятора, используя ползунки в разделе User Input: End Effector. Подсистема обратной кинематики генерирует строение соединения, которая достигает желаемого положения. Затем подсистемы контроллера соединений генерируют силы крутящего момента для каждого соединения, чтобы достичь этого положения

Откройте модель.

open_system('gazeboCosimControl');

Установите шаги расчета контроллера и траектории

Ts = 0.01;
Ts_trajectory = 0.05;

Для обеспечения хорошего шага расчета эффективности контроллера является коротким. Шаг расчета траектории больше для обеспечения скорости симуляции, что характерно для задач высокого уровня, таких как датчик камеры, обратная кинематика и генерация траектории.

Осмыслите конструкцию модели

Модель Simulink состоит из четырех областей:

Эти разделы более подробно описаны ниже.

Вход пользователя

Пользовательский вход имеет 6 ползунков, которые управляют положением конечного эффектора: три для управления положением X, Y и Z в пространстве и три для управления ориентацией.

Контроль

Секция управления перемещает требуемое положение концевого эффектора к моментам в соединениях. Во-первых, Подсистема Обратной Кинематики вычисляет положения соединений, которые удовлетворяют целевому положению end-effector. Затем Подсистема Контроллера Соединений создает входные крутящие моменты привода, учитывая исходное положение соединения, скорость и ускорение и текущее состояние робота. Состояние содержит измеренное положение и скорость выхода Gazebo.

Подсистема обратной кинематики

Обратная кинематика вычисляет положение соединения для соответствующего положения эффектора конца (положение и ориентация), учитывая начальное предположение. Для быстрого сходимости установите начальное условие задержки модуля. В этой подсистеме веса однородны, что подразумевает, что решение должно придавать одинаковое значение достижению всех положений и ориентаций в желаемом положении.

Подсистема контроллера соединений

В этом примере контроллер соединений использует обратную связь и условия feedforward. Значения гравитационного крутящего момента и скорости продукта объединяются, чтобы сформировать термин feedforward. Пропорционально-производный (PD) контроллер генерирует термин обратной связи. Для получения дополнительной информации о контроллерах соединений см. «Выполните безопасное управление отслеживанием траектории с использованием блоков Robotics Manipulator».

Gazebo Pacer

Блок Gazebo Pacer обеспечивает совместное моделирование с Gazebo путем шага симуляции Gazebo синхронно с шагами Simulink. Это важно для приложений, подобных этой модели, где контроллер используется для непосредственного управления положением робота в мире Gazebo. Чтобы симуляция отражала фактическое поведение, необходимо синхронизировать измерения соединений и команды крутящего момента.

В маске блока задайте шаг расчета как Ts. Проверить подключение Gazebo можно с помощью ссылки Настройка сети Gazebo и настройки симуляции. Это подключение уже настроено с использованием gzinit.

Модель Gazebo

Подсистема Gazebo Model содержит блоки, используемые для связи с миром Gazebo. В этом случае контроллер прикладывает крутящий момент к каждому соединению и считывает назад положение и скорость соединения.

Применить крутящий момент в соединении

Для каждого соединения крутящий момент должен применяться в подсистемах Set < имя > зоны Передачи в Gazebo подсистемы модели Gazebo. Для примера, для панорамирования обочины, инструменты для отправки подсистемы плана обочины содержатся в подсистеме Задать панорамирование обочины.

Каждая из этих подсистем использует блок Bus Assignment, входы которого определяют тип шины, детали модели и длительность и полученную команду.

Блок Gazebo Blank Message определяет тип шины как формат сообщения Gazebo соответствующего типа. В диалоговом окне параметров блоков выберите «ApplyJointTorque» из списка типов команд. Блок необходим, чтобы убедиться, что используется правильный шаблон шины, то есть, что порт является блоком назначения шины, имеют правильные значения.

Блок Gazebo Select Entity выбирает объект, к которому будет применено это сообщение. В этом случае, поскольку прикладывается крутящий момент, «сущность» является соединением. Поскольку эта подсистема применяет крутящий момент к плечевому шарниру, выберите соответствующее соединение в мире Gazebo из списка типов сущностей. Пока установлено соединение с миром Gazebo, этот список будет автоматически заселяться. Если подключение потеряно, щелкните ссылку «Configure Gazebo network and simulation settings», чтобы восстановить или использовать gzinit интерфейс командной строки. Мир Gazebo должен быть открыт для успеха этой операции.

Блок Gazebo Apply Command забирает содержимое сообщения отправляет его на сервер Gazebo. Откройте параметры диалогового окна блока, выберите параметр Command type и выберите ApplyJointTorque, чтобы отправить соответствующую команду в Gazebo.

Остальные входы определяют величину и длительность приложенной силы. Входной порт усилия определяет величину крутящего момента в соединении. Входной параметр индекса определяет ось, к которой должен применяться крутящий момент. Поскольку каждое из шарниров вращения составляет всего 1 степень свободы, это значение должно быть установлено на uint32(0) для указания первой оси. Наконец, входы длительности определяют длительность приложенного крутящего момента, разделенного на секунды и наносекунды. Для примера, если длительность составляет 1,005 секунд, это было бы 1 секунду и 5 000 000 наносекунд в качестве шины. Они разделены для повышения точности. В этом случае контроллер применяет крутящий момент для длины шага расчета, предварительно заданной как 0,01 секунды, или 1e7 наносекунд. Поэтому первый вход равен нулю (0 секунд), а второй вход равен 1e7.

Подсистема Gazebo Model включает 5 других подсистем, соответствующих остальным пяти соединениям, которые были сконфигурированы таким же образом.

Измерение состояния соединения

Вторая часть интерфейса Gazebo Model требует считывания значений от Gazebo с помощью блока Gazebo Read для каждого соединения. Для примера, блок для плечевого панорамирования показан ниже.

Блок Gazebo Read считывает сообщения с сервера Gazebo. В диалоговом окне блока нажмите «Select».. рядом с темой, чтобы выбрать правильную тему для чтения и выбрать соответствующее измеренное значение соединения /gazebo/ground_truth/joint_state/ur10/shoulder_pan_joint. Это сообщение выбирает точное значение основной истины, хотя возможно также разместить датчики в Gazebo и считать их вместо них.

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

В этом разделе также рассматривается значение красного ящика в Gazebo. Хотя возможно также задать положение этого ящика с помощью формата, аналогичного используемому выше, проще использовать интерфейс командной строки gzlink для обновления блока в дискретные моменты во время выполнения модели.

Настройте объекты шины для симуляции

Объекты шины Gazebo генерируются, когда блоки косимуляции введены в модель. Сигналы от Gazebo считываются через эти автобусные объекты. По умолчанию размерности устанавливаются на переменный размер, поскольку соединения могут иметь несколько степеней свободы. Поскольку все соединения в этом примере являются шарнирными соединениями, они являются 1-мерными и фиксированными в размере, что может быть задано как свойство сигналов шины. Этот шаг является необязательным, но он полезен, чтобы преодолеть ограничения сигналов переменного размера.

Загрузите пользовательские объекты шины.

load('custom_busobjects_basic');   

Для этого вручную откройте Редактор шины в Simulink. Это можно найти в разделе Моделирования > Проектом > Bus Редактора.

Выберите шину для чтения состояния соединения, затем выберите соответствующее имя элемента для правой оси. Для Gazebo_SL_Bus_gazebo_msgs_JointState выберите joint_position и joint_velocity и измените размерность на 1 и DimensionMode на 'Fixed'.

Запуск симуляции

Сбросьте мир Gazebo и положение коробки перед симуляцией с помощью команд MATLAB. Такие команды могут быть более непосредственно включены в модель Simulink как StartFcn коллбэк, чтобы убедиться, что они выполняются при каждом запуске симуляции. В параметры блоков Gazebo Pacer только время симуляции сбрасывается при запуске модели, если вы не измените выпадающему списку Сброс поведения.

gzworld("reset"); % Reset the world to its initial state
gzlink("set","redBox","link","Position",[0.5 -0.4 .3]); % Move the box to a new location

STATUS:  Succeed


MESSAGE:  Parameter set successfully.

Можно также запустить эти типы команд во время симуляции. Для этого используйте зеленую кнопку «run», чтобы симулировать модель Simulink, а не команду sim, чтобы убедиться, что командная строка может быть выполнена во время симуляции.

Запустите симуляцию в течение 20 секунд и протестируйте различные положения с помощью ползунков в User Input: End Effector. Проверьте правильность работы обратной кинематики и контроллера соединений в Gazebo.

simoutput = sim('gazeboCosimControl','StopTime','20');

Чтобы увидеть поведение положений соединений, используйте Данные моделирования Inspector. Это изображение показывает данные, когда командные положения манипулятора были изменены несколько раз в течение симуляции.

Просмотр эффективности

Измерения соединений и ссылки регистрируются. Когда симуляция будет завершена, постройте график записанных выходов.

measuredPosition = simoutput.logsout{1}.Values; 
referencePosition = simoutput.logsout{2}.Values; 
figure
plot(measuredPosition.Time, measuredPosition.Data, '-', referencePosition.Time, referencePosition.Data, '--')
legend({'Meas1','Meas2','Meas3','Meas4','Meas5','Meas6','Ref1','Ref2','Ref3','Ref4','Ref5','Ref6'})