Difference between revisions of "Modelo para simulación brazo MYRAbot (urdf+gazebo)"
From robotica.unileon.es
(→Programa interfaz (gazebo-programas de control)) |
(→Programa interfaz (gazebo-programas de control)) |
||
| Line 1,218: | Line 1,218: | ||
<syntaxhighlight>roscd brazo_fer_modelo | <syntaxhighlight>roscd brazo_fer_modelo | ||
make</syntaxhighlight> | make</syntaxhighlight> | ||
| + | |||
| + | ==Simulación de programas de control== | ||
| + | |||
| + | Lo primero vamos a modificar el ''launcher'' "brazo_gazebo.launch", creado previamente, para añadir el programa interfaz entre los programas de control y el modelo del brazo. A continuación se muestra el contenido del archivo modificado: | ||
| + | |||
| + | <syntaxhighlight lang="xml" enclose="div"> | ||
| + | |||
| + | <launch> | ||
| + | |||
| + | <include file="$(find gazebo_worlds)/launch/empty_world.launch"/> | ||
| + | |||
| + | <param name="robot_description" command="$(find xacro)/xacro.py '$(find brazo_fer_modelo)/brazo.urdf.xacro'" /> | ||
| + | |||
| + | <node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot_arm" respawn="false" output="screen" /> | ||
| + | |||
| + | <rosparam file="$(find brazo_fer_modelo)/controllers.yaml" command="load"/> | ||
| + | |||
| + | <node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" /> | ||
| + | |||
| + | <node name="control_modelo" pkg="brazo_fer_modelo" type="control_modelo" /> | ||
| + | |||
| + | </launch> | ||
| + | |||
| + | </syntaxhighlight> | ||
| + | |||
| + | ===Primer programa de control (Ven aquí)=== | ||
| + | |||
| + | |||
<!--http://youtu.be/S9kUKYysm0M--> | <!--http://youtu.be/S9kUKYysm0M--> | ||
Revision as of 10:24, 6 November 2013
Contents
Creación del modelo URDF
El formato empleado para el modelo del brazo está denominado por el acrónimo inglés URDF (Unified Robot Description Format), que emplea el lenguaje xml. Consiste en un árbol de elementos geométricos (links) conectados entre sí mediante uniones (joints) que determinan el parentesco entre ellos. Estas uniones pueden ser fijas o móviles, las móviles pueden ser a su vez rotacionales o lineales.
Comenzaremos creando el package "brazo_fer_modelo" que va a contener el modelo de nuestro brazo, con las siguientes pependencias: urdf brazo_fer std_msgs sensor_msgs tf roscpp
El modelo planteado para nuestro brazo es el siguiente, que guardaremos en el directorio "urdf" del package creado en un archivo llamado "brazo.urdf.xacro":
<?xml version="1.0"?>
<robot name="MYRAbot_arm"
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="default_inertia">
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</xacro:macro>
<xacro:macro name="default_inertia_servos">
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</xacro:macro>
<xacro:macro name="default_dynamics">
<dynamics fricction="0" damping="0" />
</xacro:macro>
<link name="world" />
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
<link name="base_link">
<visual>
<geometry>
<box size="0.032 0.05 0.04"/>
</geometry>
<origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
<geometry>
<box size="0.032 0.05 0.04"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
<mass value="0.055"/>
<xacro:default_inertia_servos />
</inertial>
</link>
<joint name="base" type="revolute">
<parent link="base_link"/>
<child link="hombro_link"/>
<origin xyz="0 0 0.04" rpy="0 0 0" />
<axis xyz="0 0 1" />
<limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>
<xacro:default_dynamics />
</joint>
<link name="hombro_link">
<visual>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.020"/>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.020"/>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.020"/>
<mass value="0.055"/>
<xacro:default_inertia_servos />
</inertial>
</link>
<joint name="arti1" type="revolute">
<parent link="hombro_link"/>
<child link="brazo_link"/>
<origin xyz="0 0 0.0385" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>
<xacro:default_dynamics />
</joint>
<!--brazo-->
<xacro:macro name="servo" params="nombre">
<link name="${nombre}_link">
<visual>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.0905"/>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0905"/>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0905"/>
<mass value="0.055"/>
<xacro:default_inertia_servos />
</inertial>
</link>
</xacro:macro>
<xacro:servo nombre="brazo" />
<joint name="servo_arti1_B" type="fixed">
<parent link="brazo_link"/>
<child link="brazo_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<xacro:macro name="base" params="nombre">
<link name="${nombre}_link_B">
<visual>
<geometry>
<box size="0.04 0.032 0.015"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.058"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.058"/>
<geometry>
<box size="0.04 0.032 0.015"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.058"/>
<mass value="0.001"/>
<xacro:default_inertia />
</inertial>
</link>
</xacro:macro>
<xacro:base nombre="brazo" />
<joint name="servo_arti1_D" type="fixed">
<parent link="brazo_link"/>
<child link="brazo_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<xacro:macro name="soporte" params="nombre simetrico lado">
<link name="${nombre}_link_S${lado}">
<visual>
<geometry>
<box size="0.002 0.032 0.067"/>
</geometry>
<origin rpy="0 0 0" xyz="${simetrico*0.021} 0 0.022"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="${simetrico*0.021} 0 0.022"/>
<geometry>
<box size="0.002 0.032 0.067"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${simetrico*0.021} 0 0.022"/>
<mass value="0.001"/>
<xacro:default_inertia />
</inertial>
</link>
</xacro:macro>
<xacro:soporte nombre="brazo" simetrico="1" lado="I" />
<joint name="servo_arti1_I" type="fixed">
<parent link="brazo_link"/>
<child link="brazo_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<xacro:soporte nombre="brazo" simetrico="-1" lado="D" />
<joint name="arti2" type="revolute">
<parent link="brazo_link"/>
<child link="antebrazo_link"/>
<origin xyz="0 0 0.104" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>
<xacro:default_dynamics />
</joint>
<!--antebrazo-->
<xacro:servo nombre="antebrazo" />
<joint name="servo_arti2_B" type="fixed">
<parent link="antebrazo_link"/>
<child link="antebrazo_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<xacro:base nombre="antebrazo" />
<joint name="servo_arti2_D" type="fixed">
<parent link="antebrazo_link"/>
<child link="antebrazo_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<xacro:soporte nombre="antebrazo" simetrico="1" lado="I" />
<joint name="servo_arti2_I" type="fixed">
<parent link="antebrazo_link"/>
<child link="antebrazo_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<xacro:soporte nombre="antebrazo" simetrico="-1" lado="D" />
<joint name="arti3" type="revolute">
<parent link="antebrazo_link"/>
<child link="muneca_link"/>
<origin xyz="0 0 0.104" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit effort="1000.0" lower="-2.62" upper="2.62" velocity="2"/>
<xacro:default_dynamics />
</joint>
<!--muneca-->
<link name="muneca_link">
<visual>
<geometry>
<box size="0.05 0.04 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
<geometry>
<box size="0.05 0.04 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
<mass value="0.055"/>
<xacro:default_inertia_servos />
</inertial>
</link>
<joint name="servo_arti3_B" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_B">
<visual>
<geometry>
<box size="0.05325 0.04 0.005"/>
</geometry>
<origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
<geometry>
<box size="0.05325 0.04 0.005"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_arti3_D" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_SI">
<visual>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
<origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_arti3_I" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_SD">
<visual>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_arti3_P" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_P"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<!--pinza izquierda fija-->
<link name="muneca_link_P">
<visual>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
<origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
<mass value="0.001"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_arti3_PS" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_PS"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_PS">
<visual>
<geometry>
<box size="0.002 0.04 0.037"/>
</geometry>
<origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.03225 0 0.060"/>
<geometry>
<box size="0.002 0.04 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.03225 0 0.060"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="pinza" type="revolute">
<parent link="muneca_link"/>
<child link="pinza_link"/>
<origin xyz="-0.00725 0 0.06" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>
<xacro:default_dynamics />
</joint>
<!--pinza-->
<link name="pinza_link">
<visual>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
<mass value="0.001"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_pinza_B" type="fixed">
<parent link="pinza_link"/>
<child link="pinza_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="pinza_link_B">
<visual>
<geometry>
<box size="0.002 0.04 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.025 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.025 0 0"/>
<geometry>
<box size="0.002 0.04 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.025 0 0"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_pinza_D" type="fixed">
<parent link="pinza_link"/>
<child link="pinza_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="pinza_link_SI">
<visual>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
<joint name="servo_pinza_I" type="fixed">
<parent link="pinza_link"/>
<child link="pinza_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="pinza_link_SD">
<visual>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
<mass value="0.0005"/>
<xacro:default_inertia />
</inertial>
</link>
</robot>
Análisis del modelo
Como estamos empleando xacro para simplificar y hacer más editable el código necesitamos hacer la conversión a URDF para obtener el archivo procesado, ejecutando en un terminal la siguiente secuencia de comandos:
roscd brazo_fer_modelo
cd urdf
rosrun xacro xacro.py brazo.urdf.xacro > brazo.urdfAhora podemos comprobar si el modelo que hemos creado es correcto ejecutando el siguiente comando en el terminal:
rosrun urdf_parser check_urdf brazo.urdfSi todo va bien deberiamos obtener lo siguiente:
robot name is: MYRAbot_arm
---------- Successfully Parsed XML ---------------
root Link: world has 1 child(ren)
child(1): base_link
child(1): hombro_link
child(1): brazo_link
child(1): antebrazo_link
child(1): muneca_link
child(1): pinza_link
child(1): pinza_link_B
child(2): pinza_link_SI
child(3): pinza_link_SD
child(2): muneca_link_B
child(3): muneca_link_SI
child(4): muneca_link_SD
child(5): muneca_link_P
child(6): muneca_link_PS
child(2): antebrazo_link_B
child(3): antebrazo_link_SI
child(4): antebrazo_link_SD
child(2): brazo_link_B
child(3): brazo_link_SI
child(4): brazo_link_SDPodemos ver el árbol de relaciones de forma gráfica generando un archivo pdf, para lo que ejecutaremos en el terminal el siguiente comando:
rosrun urdf_parser urdf_to_graphiz brazo.urdfEl resultado obtenido será el siguiente:
Prueba visual del modelo
Para poder visualizar el modelo y comprobar el correcto funcionamiento de las uniones joints vamos a crear un launcher para cargarlo en el robot_state_publisher y ejecutar el joint_state_publisher con el interfaz de publicación del estado de las uniones móviles, y el rviz para la visualización. Crearemos el archivo "brazo_rviz.launch" en el directorio "launch" del package que hemos creado, con el siguiente contenido:
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find brazo_fer_modelo)/urdf/brazo.urdf.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<param name="use_gui" value="true"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" />
</launch>
Para iniciar el launcher debemos ejecutar en un terminal el siguiente comando:
roslaunch brazo_fer_modelo brazo_rviz.launchSi todo va bien, se ejecutará rviz y el interfaz del joint_state_publisher, sobre el cual al modificar los controles deslizables observaremos como se modifica la posición de las uniones móviles en el modelo 3D cargado en rviz. Para que se muestre correctamente nuestro modelo en rviz debemos seleccionar el elemento link que se va ha tomar como elemento fijo "fixed frame", en nuestro caso en "Global Options" el "Fixed Frame" será el link "/world". El resultado es el que se muestra a continuación:
Modelo cargado en rviz e interfaz joint_state_publisher
Control de uniones para simulación
Una vez creado el modelo URDF y probada la correcta representación y funcionamiento de las uniones móviles, debemos añadir una serie de controladores a cada unión para poder simular en gazebo los servomotores Dynamixel AX-12A.
Pasos previos
Debemos comenzar añadiendo los packages donde se encuentran los controladores que vamos a emplear. Se trata de los controladores del PR2 por lo que necesitamos instalar los stacks pr2_simulator, pr2_mechanism y pr2_controllers. Para ello tenemos que ejecutar la siguiente secuencia de comandos en un terminal:
sudo atp-get update
sudo apt-get install ros-VERSIÓN_ROS-pr2-simulator ros-VERSIÓN_ROS-pr2-mechanism ros-VERSIÓN_ROS-pr2-controllersAñadir controladores a uniones móviles
Modificación del URDF
En el URDF hemos definido la geometría y propiedades físicas de los elementos (link), así como las uniones (joint) entre estos y sus características. Para poder visualizar el color que le hemos dado a cada elemento en gazebo debemos asignar a cada elemento un material gazebo. También necesitamos establecer los coeficientes de rozamiento de cada elemento, establecer si pueden colisionar los elementos entre sí y la acción de la fuerza de gravedad. A continuación se muestra la descripción para cada elemento en gazebo, que añadiremos al final de nuestro URDF antes de la etiqueta </robot>:
<xacro:macro name="default_mu">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
</xacro:macro>
<gazebo reference="base_link">
<material>Gazebo/Black</material>
<xacro:default_mu />
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="hombro_link">
<xacro:default_mu />
<material>Gazebo/Black</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="brazo_link">
<xacro:default_mu />
<material>Gazebo/Black</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="antebrazo_link">
<xacro:default_mu />
<material>Gazebo/Black</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="muneca_link">
<xacro:default_mu />
<material>Gazebo/Black</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="brazo_link_B">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="brazo_link_SI">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="brazo_link_SD">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="antebrazo_link_B">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="antebrazo_link_SI">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="antebrazo_link_SD">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="muneca_link_B">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="muneca_link_SI">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="muneca_link_SD">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="muneca_link_P">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="muneca_link_PS">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="pinza_link">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="pinza_link_B">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="pinza_link_SI">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="pinza_link_SD">
<xacro:default_mu />
<material>Gazebo/White</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
Ahora nos queda añadir las características de las uniones móviles, el plugin del controlador que vamos a emplear para el control de estas y el modelado del mecanismo de transmisión. A continuación se muestra el código que debemos añadir al final de nuestro fichero URDF antes de la etiqueta </robot>:
<gazebo reference="base">
<erp>0.1</erp>
<stopKd value="100000.0" />
<stopKp value="100000.0" />
<fudgeFactor value="0.5" />
</gazebo>
<gazebo reference="arti1">
<erp>0.1</erp>
<stopKd value="100000.0" />
<stopKp value="100000.0" />
<fudgeFactor value="0.5" />
</gazebo>
<gazebo reference="arti2">
<erp>0.1</erp>
<stopKd value="100000.0" />
<stopKp value="100000.0" />
<fudgeFactor value="0.5" />
</gazebo>
<gazebo reference="arti3">
<erp>0.1</erp>
<stopKd value="100000.0" />
<stopKp value="100000.0" />
<fudgeFactor value="0.5" />
</gazebo>
<gazebo reference="pinza">
<erp>0.1</erp>
<stopKd value="100000.0" />
<stopKp value="100000.0" />
<fudgeFactor value="0.5" />
</gazebo>
<gazebo>
<controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
</controller:gazebo_ros_controller_manager>
</gazebo>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="base_trans">
<actuator name="base_motor" />
<joint name="base" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="arti1_trans">
<actuator name="arti1_motor" />
<joint name="arti1" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="arti2_trans">
<actuator name="arti2_motor" />
<joint name="arti2" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="arti3_trans">
<actuator name="arti3_motor" />
<joint name="arti3" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="pinza_trans">
<actuator name="pinza_motor" />
<joint name="pinza" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>Creación del fichero de configuración
Los controladores que vamos a emplear para cada unión móvil son uno de posición y otro de velocidad. Cada controlador esta modelado por un regulador PID. Debemos crear un archivo dentro de nuestro package llamado "controllers.yaml", con la configuración de los controladores de cada unión móvil, cuyo contenido será el siguiente:
base_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: base
pid: &base_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
arti1_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: arti1
pid: &arti1_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
arti2_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: arti2
pid: &arti2_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
arti3_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: arti3
pid: &arti3_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
pinza_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: pinza
pid: &pinza_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
base_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: base
pid: &base_vel
p: 2.0
i: 0.5
i_clamp: 10.0
arti1_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: arti1
pid: &arti1_vel
p: 2.0
i: 0.5
i_clamp: 10.0
arti2_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: arti2
pid: &arti2_vel
p: 2.0
i: 0.5
i_clamp: 10.0
arti3_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: arti3
pid: &arti3_vel
p: 2.0
i: 0.5
i_clamp: 10.0
pinza_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: pinza
pid: &pinza_vel
p: 2.0
i: 0.5
i_clamp: 10.0Carga del modelo en gazebo
Para poder mostrar el modelo en gazebo vamos a crear un launcher que cargue el modelo un mundo vacío y los controladores de las uniones móviles. Crearemos el archivo "brazo_gazebo.launch" en el directorio "launch" de nuestro package, con el siguiente contenido:
<launch>
<include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find brazo_fer_modelo)/brazo.urdf.xacro'" />
<node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot_arm" respawn="false" output="screen" />
<rosparam file="$(find brazo_fer_modelo)/controllers.yaml" command="load"/>
<node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
</launch>
Si todo va bien podremos ver en la ventana gŕfica de gazebo el modelo del brazo en posición vertical situado en el punto xyz = (0 0 0) de un mundo vacío, tal como se muestra en la siguiente imagen:
Modelo brazo MYRAbot en gazebo
Podemos comprobar que se han cargado correctamente todos los controladores de las uniones móviles, ya que tienen que aparecer dos topics por cada controlador, para lo que ejecutaremos en un terminal el siguiente comando:
rostopic echoDebería aparecernos algo similar a lo que se muestra a continuación, donde el topic NOMBRE_CONTROLADOR/command es al que está suscrito el controlador para recibir el valor de consigna y el topic NOMBRE_CONTROLADOR/status es en el que publica el estado del controlador:
/arti1_pos_controller/command
/arti1_pos_controller/state
/arti1_vel_controller/command
/arti1_vel_controller/state
/arti2_pos_controller/command
/arti2_pos_controller/state
/arti2_vel_controller/command
/arti2_vel_controller/state
/arti3_pos_controller/command
/arti3_pos_controller/state
/arti3_vel_controller/command
/arti3_vel_controller/state
/base_pos_controller/command
/base_pos_controller/state
/base_vel_controller/command
/base_vel_controller/state
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/paused
/gazebo/set_link_state
/gazebo/set_model_stat
/joint_states
/mechanism_statistics
/pinza_pos_controller/command
/pinza_pos_controller/state
/pinza_vel_controller/command
/pinza_vel_controller/state
/rosout
/rosout_agg
/tfPrograma interfaz (gazebo-programas de control)
Al igual que en el brazo real, que empleabamos una placa arduino que contenía el programa de interfaz entre el brazo y nuestros programas de control, hemos creado un programa que se suscriba a los topic que publican los programas de control y publique en consecuencia en los topics a los que están suscritos los controladores de las uniones móviles. Crearemos un archivo con el nombre "control_modelo.cpp" en el directorio "src" de nuestro package con el siguiente contenido:
#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
#include "math.h"
#define PI 3.14159265
void mover(const brazo_fer::WriteServos& move)
{
ros::NodeHandle n;
ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("base_pos_controller/command", 1);
ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("arti1_pos_controller/command", 1);
ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("arti2_pos_controller/command", 1);
ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("arti3_pos_controller/command", 1);
ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("base_vel_controller/command", 1);
ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("arti1_vel_controller/command", 1);
ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("arti2_vel_controller/command", 1);
ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("arti3_vel_controller/command", 1);
brazo_fer::Servos position = move.posicion;
brazo_fer::Servos speed = move.velocidad;
brazo_fer::Servos torque = move.par;
std_msgs::Float64 base_pos_command, arti1_pos_command, arti2_pos_command, arti3_pos_command;
std_msgs::Float64 base_vel_command, arti1_vel_command, arti2_vel_command, arti3_vel_command;
if (torque.base != 0)
{
base_pos_command.data = ((((position.base * 300)/1023)*PI)/180)-2.62;
base_vel_command.data = speed.base/511;
}
if (torque.arti1 != 0)
{
arti1_pos_command.data = ((((position.arti1*300)/1023)*PI)/180)-2.62;
arti1_vel_command.data = speed.arti1/511;
}
if (torque.arti2 != 0)
{
arti2_pos_command.data = ((((position.arti2*300)/1023)*PI)/180)-2.62;
arti2_vel_command.data = speed.arti2/511;
}
if (torque.arti3 != 0)
{
arti3_pos_command.data = ((((position.arti3*300)/1023)*PI)/180)-2.62;
arti3_vel_command.data = speed.arti3/511;
}
base_pos_pub_.publish(base_pos_command);
arti1_pos_pub_.publish(arti1_pos_command);
arti2_pos_pub_.publish(arti2_pos_command);
arti3_pos_pub_.publish(arti3_pos_command);
base_vel_pub_.publish(base_vel_command);
arti1_vel_pub_.publish(arti1_vel_command);
arti2_vel_pub_.publish(arti2_vel_command);
arti3_vel_pub_.publish(arti3_vel_command);
}
void pinza(const brazo_fer::WriteServos& pinza)
{
ros::NodeHandle n;
ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("pinza_pos_controller/command", 1);
ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("pinza_vel_controller/command", 1);
brazo_fer::Servos position = pinza.posicion;
brazo_fer::Servos speed = pinza.velocidad;
brazo_fer::Servos torque = pinza.par;
std_msgs::Float64 pinza_pos_command;
std_msgs::Float64 pinza_vel_command;
if (torque.pinza != 0)
{
pinza_pos_command.data = ((((position.pinza*300)/1023)*PI)/180)-2.62;
pinza_vel_command.data = speed.pinza/511;
}
pinza_pos_pub_.publish(pinza_pos_command);
pinza_vel_pub_.publish(pinza_vel_command);
}
void pe(const sensor_msgs::JointState& pe)
{
ros::NodeHandle n;
ros::Publisher pose_pub_=n.advertise<brazo_fer::ReadServos>("pose_arm", 1);
brazo_fer::ReadServos estado;
estado.posicion.base = ((((pe.position[0]+2.62)*180)/PI)*1023)/300;
estado.posicion.arti1 = ((((pe.position[1]+2.62)*180)/PI)*1023)/300;
estado.posicion.arti2 = ((((pe.position[2]+2.62)*180)/PI)*1023)/300;
estado.posicion.arti3 = ((((pe.position[3]+2.62)*180)/PI)*1023)/300;
estado.posicion.pinza = ((((pe.position[4]+2.62)*180)/PI)*1023)/300;
estado.corriente.base = pe.effort[0]*1000;
estado.corriente.arti1 = pe.effort[1]*1000;
estado.corriente.arti2 = pe.effort[2]*1000;
estado.corriente.arti3 = pe.effort[3]*1000;
estado.corriente.pinza = pe.effort[4]*1000;
pose_pub_.publish(estado);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "modelo_brazo");
ros::NodeHandle n;
ros::Subscriber move_sub_= n.subscribe("move_arm", 1, mover);
ros::Subscriber hand_sub_= n.subscribe("hand_arm", 1, pinza);
ros::Subscriber pe_sub_= n.subscribe("joint_states", 1, pe);
ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("base_pos_controller/command", 1);
ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("arti1_pos_controller/command", 1);
ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("arti2_pos_controller/command", 1);
ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("arti3_pos_controller/command", 1);
ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("pinza_pos_controller/command", 1);
ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("base_vel_controller/command", 1);
ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("arti1_vel_controller/command", 1);
ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("arti2_vel_controller/command", 1);
ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("arti3_vel_controller/command", 1);
ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("pinza_vel_controller/command", 1);
ros::Publisher pose_pub_=n.advertise<brazo_fer::ReadServos>("pose_arm", 1);
ros::spin();
return 0;
}
Para compilarlo y generar el ejecutable se debe añadir la siguiente línea de código al archivo "CMakeLists.txt" del package creado, donde indicamos el nombre para el ejecutable y la ruta y nombre del archivo a compilar:
rosbuild_add_executable(control_modelo src/control_modelo.cpp)Para compilar el programa hay que situarse en el directorio del package. Simplemente con ejecutar la siguiente secuencia de comandos en un terminal se compilará y creará el ejecutable, siempre que no existan errores:
roscd brazo_fer_modelo
makeSimulación de programas de control
Lo primero vamos a modificar el launcher "brazo_gazebo.launch", creado previamente, para añadir el programa interfaz entre los programas de control y el modelo del brazo. A continuación se muestra el contenido del archivo modificado:
<launch>
<include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find brazo_fer_modelo)/brazo.urdf.xacro'" />
<node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot_arm" respawn="false" output="screen" />
<rosparam file="$(find brazo_fer_modelo)/controllers.yaml" command="load"/>
<node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
<node name="control_modelo" pkg="brazo_fer_modelo" type="control_modelo" />
</launch>
