Gazebo - это симулятор механических систем, и прежде всего роботов. Он позволяет учесть не только геометрию модели, но также и её динамику. Это бесплатная программа с открытым исходным кодом. Gazebo не является частью ROS, но глубоко интегрирован с этой системой, и обычно входит в full desctop версию. Когда-нибудь я напишу об этой программе подробнее, пока же достаточно знать, что она есть.
Поскольку Gazebo учитывает динамику, модель робота должна быть дополнена соответствующими физическими параметрами. Кроме того, инерционные эффекты приведут к отклонению реального движения робота от желаемого. Следовательно, в модель должна быть включена система управления, которая эти отклонения скомпенсирует. Данный пример был подготовлен на основе следующего руководства.
Для начала, создадим новую ноду scara_control со всеми необходимыми зависимостями.
Для начала, создадим новую ноду scara_control со всеми необходимыми зависимостями.
cd ~/my_ros_ws
catkin_create_pkg scara_control controller_manager joint_state_controller robot_state_publisher
cd scara_control
mkdir config launch urdf
Здесь robot_state_publisher публикует текущее состояние (координаты) робота, joint_state_controller осуществляет управление положением звеньев, а controller_manager предоставляет интерфейс для взаимодействия с компонентами робота в реальном времени.
Скопируем в папку scara_control/urdf описание модели, которое было сделано в одном из предыдущих постов, и внесём в него ряд дополнений и изменений. Прежде всего, необходимо "прикрепить" робота к земле
<link name="world" />
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
Кроме того, в прошлой модели система координат звена base_link находилась в его центре масс, в результате, звено было наполовину погружено в землю. Теперь его нужно "приподнять", иначе модель будет "скакать" из-за попыток симулятора вытолкнуть начальное звено из земли. Сделать это можно за счёт добавления тега
<origin xyz="0 0 0.25" />
в описание звена base_link, и соответствующего изменения смещения в сочленении base_to_link1
<origin xyz="0 0 0.535" />
С исправлением ошибок покончено, перейдём к дополнению модели. Для каждого физически реального звена необходимо включить информацию о коллизии и инерции. Например, для второго звена результат будет следующим:
<link name="link2">
<collision>
<geometry>
<box size="0.06 0.06 0.4" />
</geometry>
<origin rpy="0 1.57075 0" xyz="-0.2 0 0" />
</collision>
<visual>
<geometry>
<box size="0.06 0.06 0.4" />
</geometry>
<material name="white" />
<origin rpy="0 1.57075 0" xyz="-0.2 0 0" />
</visual>
<inertial>
<mass value="1.0" />
<origin rpy="0 1.57075 0" xyz="-0.2 0 0" />
<inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0" />
</inertial>
</link>
Блок <intertial> ... </inertial> содержит физические параметры звена, в частности, массу и элементы матрицы инерции. Я поставил везде единицы, хотя, конечно, здесь должны быть значения, зависящие от материала и формы. Что касается суставов, добавим для каждого из них значение
<dynamics damping="0.7" />
Это необходимо для того, чтобы колебания не продолжались бесконечно даже при отсутствии системы управления. Для осуществления движения нам необходимо добавить двигатели в соответствующие суставы. Сделать это можно с помощью блоков.
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_link1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link1_to_link2">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link2_to_link3">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
Для каждой системы передачи мы определяем тип (здесь используется SimpleTransmission), характер управления (EffortJointInterface - на основе усили), соответствующее сочленение и передаточное число. Наконец, необходимо подключить плагин для связи ROS и Gazebo:
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/scara</robotNamespace>
</plugin>
</gazebo>
Чтобы настроить систему управления, перейдём в директорию scara_control/config и создадим файл scara_control.yaml со следующим содержанием
Первая строка здесь задаёт пространство имён, joint_state_controller публикует данные о состоянии робота (с частотой 50 раз в секунду), а jointN_position_controller выставляет нужную позицию в соответствующем сочленении. Управление происходит на основе ПИД-регулятора, параметры которого здесь определены.
Далее, перейдём в scara_control/launch и создадим файл scara_control.launch для подключения системы управления к нашей модели. Данный файл будет загружать конфигурацию системы управления, описанную в yaml-файле, саму систему управления, а также ноду для публикации состояния робота. Кроме того, он преобразует пространство имён в /scara.
<launch>
<!-- загрузка конфигурации контроллера -->
<rosparam file="$(find scara_control)/config/scara_control.yaml" command="load" />
<!-- загрузка контроллеров -->
<node name="controller_spawner" pkg="controller_manager"
type="spawner" respawn="false" output="screen" ns="/scara"
args="joint1_position_controller joint2_position_controller joint3_position_controller joint_state_controller"/>
<!-- трансформации координат -->
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" respawn="false" output="screen">
<remap from="/joint_states" to="/scara/joint_states" />
</node>
</launch>
Для запуска Gazebo с созданной моделью используем ещё один launch-файл, назовём его scara_world.launch. Содержимое файла я позаимствовал из пакета gazebo_ros_demos. Единственный блок, который нуждался в изменении, это передача URDF модели серверу параметров ROS.
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Загрузка URDF в ROS Parameter Server -->
<param name="robot_description" textfile="$(find scara_control)/urdf/scara.urdf" />
<!-- Запуск скрипта на python -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model scara -param robot_description"/>
</launch>
Осталось сделать сборку ноды, и можно проверить, что получилось. Для этого запустим Gazebo и систему управления с помощью комманд
roslaunch scara_control scara_world.launch
roslaunch scara_control scara_control.launch
В результате, должно появиться следующее окно
Для задания какого-либо угла можно послать соответствующую команду на топик /scara/jointN_position_control/command прямо из консоли. Однако, удобнее работать через графическое окно. Запустим rqt_gui:
<link name="world" />
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
Кроме того, в прошлой модели система координат звена base_link находилась в его центре масс, в результате, звено было наполовину погружено в землю. Теперь его нужно "приподнять", иначе модель будет "скакать" из-за попыток симулятора вытолкнуть начальное звено из земли. Сделать это можно за счёт добавления тега
<origin xyz="0 0 0.25" />
в описание звена base_link, и соответствующего изменения смещения в сочленении base_to_link1
<origin xyz="0 0 0.535" />
С исправлением ошибок покончено, перейдём к дополнению модели. Для каждого физически реального звена необходимо включить информацию о коллизии и инерции. Например, для второго звена результат будет следующим:
<link name="link2">
<collision>
<geometry>
<box size="0.06 0.06 0.4" />
</geometry>
<origin rpy="0 1.57075 0" xyz="-0.2 0 0" />
</collision>
<visual>
<geometry>
<box size="0.06 0.06 0.4" />
</geometry>
<material name="white" />
<origin rpy="0 1.57075 0" xyz="-0.2 0 0" />
</visual>
<inertial>
<mass value="1.0" />
<origin rpy="0 1.57075 0" xyz="-0.2 0 0" />
<inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0" />
</inertial>
</link>
Блок <intertial> ... </inertial> содержит физические параметры звена, в частности, массу и элементы матрицы инерции. Я поставил везде единицы, хотя, конечно, здесь должны быть значения, зависящие от материала и формы. Что касается суставов, добавим для каждого из них значение
<dynamics damping="0.7" />
Это необходимо для того, чтобы колебания не продолжались бесконечно даже при отсутствии системы управления. Для осуществления движения нам необходимо добавить двигатели в соответствующие суставы. Сделать это можно с помощью блоков.
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_to_link1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link1_to_link2">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link2_to_link3">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
Для каждой системы передачи мы определяем тип (здесь используется SimpleTransmission), характер управления (EffortJointInterface - на основе усили), соответствующее сочленение и передаточное число. Наконец, необходимо подключить плагин для связи ROS и Gazebo:
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/scara</robotNamespace>
</plugin>
</gazebo>
Чтобы настроить систему управления, перейдём в директорию scara_control/config и создадим файл scara_control.yaml со следующим содержанием
Далее, перейдём в scara_control/launch и создадим файл scara_control.launch для подключения системы управления к нашей модели. Данный файл будет загружать конфигурацию системы управления, описанную в yaml-файле, саму систему управления, а также ноду для публикации состояния робота. Кроме того, он преобразует пространство имён в /scara.
<launch>
<!-- загрузка конфигурации контроллера -->
<rosparam file="$(find scara_control)/config/scara_control.yaml" command="load" />
<!-- загрузка контроллеров -->
<node name="controller_spawner" pkg="controller_manager"
type="spawner" respawn="false" output="screen" ns="/scara"
args="joint1_position_controller joint2_position_controller joint3_position_controller joint_state_controller"/>
<!-- трансформации координат -->
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" respawn="false" output="screen">
<remap from="/joint_states" to="/scara/joint_states" />
</node>
</launch>
Для запуска Gazebo с созданной моделью используем ещё один launch-файл, назовём его scara_world.launch. Содержимое файла я позаимствовал из пакета gazebo_ros_demos. Единственный блок, который нуждался в изменении, это передача URDF модели серверу параметров ROS.
<launch>
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Загрузка URDF в ROS Parameter Server -->
<param name="robot_description" textfile="$(find scara_control)/urdf/scara.urdf" />
<!-- Запуск скрипта на python -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model scara -param robot_description"/>
</launch>
Осталось сделать сборку ноды, и можно проверить, что получилось. Для этого запустим Gazebo и систему управления с помощью комманд
roslaunch scara_control scara_world.launch
roslaunch scara_control scara_control.launch
В результате, должно появиться следующее окно
Для задания какого-либо угла можно послать соответствующую команду на топик /scara/jointN_position_control/command прямо из консоли. Однако, удобнее работать через графическое окно. Запустим rqt_gui:
rosrun rqt_gui rqt_gui
В меню Plugins откроем Topics/Message publisher, затем в выпадающем меню Topic нужно выбрать /scara/joint1[2,3]_position_control/command и нажать на синий "плюс". Для каждого топика можно задать частоту публикации rate и публикуемое значение в поле expression. Публиковать можно и функциональную зависимость от времени, при этом в качестве переменной выступает i.
rqt_gui позволяет не только публиковать сообщения, но также строить графики координат или настраивать параметры ПИД-регулятора, которые сейчас явно далеки от оптимальных значений.
Комментариев нет:
Отправить комментарий