Difference between revisions of "Modelo para simulación brazo MYRAbot (urdf+gazebo)"

From robotica.unileon.es
Jump to: navigation, search
(→‎Pasos previos)
 
(70 intermediate revisions by 2 users not shown)
Line 1: Line 1:
 
[[Mobile manipulation | < volver a principal]]
 
[[Mobile manipulation | < volver a principal]]
  
−
==Creación del modelo URDF==
 
  
−
El formato empleado para el modelo del brazo está denominado por el acrónimo inglés [http://wiki.ros.org/urdf/Tutorials URDF] (Unified Robot Description Format), que emplea el lenguaje [http://es.wikipedia.org/wiki/Extensible_Markup_Language xml]. Consiste en un arbol de lementos geométricos (''links'') conectados entre sí mediante uniones (''joints'') que determinan el parentesco entre ellos. Estas uniones puden ser fijas o móviles, las móviles pueden ser a su vez rotacionales o lineales.
+
----
 +
{| style="border: solid 0px white; width: 100%"
 +
!Artículos realcionados
 +
----
 +
!Otros artículos
 +
----
 +
|-
 +
| valign="top" width="50%" |
  
−
Comenzaremos [[Fernando-TFM-ROS02#Creando un package|creando el package]] "brazo_fer_modelo" que va a contener el modelo de nuestro brazo, con las siguientes pedendencias: urdf [[Control brazo MYRAbot (bioloid+arduino)|brazo_fer]] std_msgs sensor_msgs tf roscpp
+
[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
 +
[[Detección y cálculo de posición de objetos (cámara web)]]<br/>
 +
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<br/>
 +
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
 +
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
  
−
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":
+
| valign="top" |
 +
 
 +
[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
 +
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
 +
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
 +
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
 +
----
 +
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
 +
 
 +
|}
 +
----
 +
 
 +
 
 +
=Creación del modelo URDF=
 +
 
 +
El formato empleado para el modelo del brazo está denominado por el acrónimo inglés [http://wiki.ros.org/urdf/Tutorials URDF] (Unified Robot Description Format), que emplea el lenguaje [http://es.wikipedia.org/wiki/Extensible_Markup_Language 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, lineales o flotantes.
 +
 
 +
Comenzaremos [[Fernando-TFM-ROS02#Creando un package|creando el package]] "brazo_fer_modelo" que va a contener el modelo de nuestro brazo, con las siguientes dependencias: urdf [[Control brazo MYRAbot (bioloid+arduino)|brazo_fer]] std_msgs sensor_msgs tf roscpp
 +
 
 +
El modelo planteado para nuestro brazo se compone de tres archivos, uno que contiene los ''macros'' que vamos a utilizar en el archivo principal del modelo y un archivo en el que marcamos el ''link'' al que está unido el modelo. El contenido del arcivo principal del modelo es el siguiente, que guardaremos en el directorio "urdf" del ''package'' creado en un archivo llamado "brazo.xacro":
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
 
<?xml version="1.0"?>
 
<?xml version="1.0"?>
  
−
<robot name="MYRAbot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
<robot>
−
 
 
−
<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">
+
<include filename="$(find brazo_fer_modelo)/urdf/brazo-macros.xacro" />
−
      <dynamics fricction="0" damping="0" />
 
−
</xacro:macro>
 
  
−
  <link name="world" />
+
<macro name="brazo" params="parent *origin">
  
 
   <joint name="fixed" type="fixed">
 
   <joint name="fixed" type="fixed">
−
     <parent link="world"/>
+
     <parent link="${parent}"/>
−
     <child link="base_link"/>
+
     <child link="base_brazo_link"/>
−
     <origin xyz="0 0 0" rpy="0 0 0" />
+
     <insert_block name="origin" />
 
     <axis xyz="0 0 1" />   
 
     <axis xyz="0 0 1" />   
 
   </joint>
 
   </joint>
 
      
 
      
−
   <link name="base_link">
+
   <link name="base_brazo_link">
 
     <visual>
 
     <visual>
 
       <geometry>
 
       <geometry>
Line 47: Line 68:
 
     <collision>
 
     <collision>
 
       <origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
 
       <origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
−
  <geometry>
+
      <geometry>
 
         <box size="0.032 0.05 0.04"/>
 
         <box size="0.032 0.05 0.04"/>
 
       </geometry>
 
       </geometry>
Line 54: Line 75:
 
       <origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
 
       <origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
 
       <mass value="0.055"/>
 
       <mass value="0.055"/>
−
  <xacro:default_inertia_servos />
+
  <default_inertia_servos />
 
     </inertial>       
 
     </inertial>       
 
   </link>
 
   </link>
  
 
   <joint name="base" type="revolute">
 
   <joint name="base" type="revolute">
−
     <parent link="base_link"/>
+
     <parent link="base_brazo_link"/>
 
     <child link="hombro_link"/>
 
     <child link="hombro_link"/>
 
     <origin xyz="0 0 0.04" rpy="0 0 0" />
 
     <origin xyz="0 0 0.04" rpy="0 0 0" />
 
     <axis xyz="0 0 1" />
 
     <axis xyz="0 0 1" />
−
    <limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>
+
<default_limit />
−
<xacro:default_dynamics />           
+
<default_dynamics />           
 
   </joint>
 
   </joint>
 
    
 
    
Line 72: Line 93:
 
         <box size="0.04 0.032 0.05"/>
 
         <box size="0.04 0.032 0.05"/>
 
       </geometry>
 
       </geometry>
−
       <origin rpy="0 0 0" xyz="0 0 0.020"/>
+
       <origin rpy="0 0 0" xyz="0 0 0.025"/>
 
       <material name="black" />
 
       <material name="black" />
 
     </visual>
 
     </visual>
 
     <collision>
 
     <collision>
−
       <origin rpy="0 0 0" xyz="0 0 0.020"/>
+
       <origin rpy="0 0 0" xyz="0 0 0.025"/>
 
  <geometry>
 
  <geometry>
 
         <box size="0.04 0.032 0.05"/>
 
         <box size="0.04 0.032 0.05"/>
Line 82: Line 103:
 
     </collision>  
 
     </collision>  
 
     <inertial>
 
     <inertial>
−
       <origin rpy="0 0 0" xyz="0 0 0.020"/>
+
       <origin rpy="0 0 0" xyz="0 0 0.025"/>
 
       <mass value="0.055"/>
 
       <mass value="0.055"/>
−
  <xacro:default_inertia_servos />
+
  <default_inertia_servos />
 
     </inertial>         
 
     </inertial>         
 
   </link>   
 
   </link>   
Line 93: Line 114:
 
     <origin xyz="0 0 0.0385" rpy="0 0 0" />
 
     <origin xyz="0 0 0.0385" rpy="0 0 0" />
 
<axis xyz="1 0 0" />
 
<axis xyz="1 0 0" />
−
    <limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>  
+
<default_limit />
−
<xacro:default_dynamics />         
+
<default_dynamics />         
 
   </joint>
 
   </joint>
 
    
 
    
 
   <!--brazo-->
 
   <!--brazo-->
  
−
<xacro:macro name="servo" params="nombre"> 
+
<servo nombre="brazo" />
−
  <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">
 
   <joint name="servo_arti1_B" type="fixed">
Line 131: Line 129:
 
   </joint>   
 
   </joint>   
  
−
<xacro:macro name="base" params="nombre">   
+
<base nombre="brazo" />  
−
  <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">
 
   <joint name="servo_arti1_D" type="fixed">
Line 164: Line 137:
 
<axis xyz="1 0 0" />     
 
<axis xyz="1 0 0" />     
 
   </joint>   
 
   </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" />   
+
<soporte nombre="brazo" simetrico="1" lado="I" />   
 
    
 
    
 
   <joint name="servo_arti1_I" type="fixed">
 
   <joint name="servo_arti1_I" type="fixed">
Line 199: Line 147:
 
   </joint>   
 
   </joint>   
 
    
 
    
−
<xacro:soporte nombre="brazo" simetrico="-1" lado="D" />     
+
<soporte nombre="brazo" simetrico="-1" lado="D" />     
 
    
 
    
 
   <joint name="arti2" type="revolute">
 
   <joint name="arti2" type="revolute">
Line 206: Line 154:
 
     <origin xyz="0 0 0.104" rpy="0 0 0" />
 
     <origin xyz="0 0 0.104" rpy="0 0 0" />
 
     <axis xyz="1 0 0" />
 
     <axis xyz="1 0 0" />
−
    <limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>  
+
<default_limit />
−
<xacro:default_dynamics />             
+
<default_dynamics />             
 
   </joint>
 
   </joint>
 
    
 
    
 
   <!--antebrazo-->
 
   <!--antebrazo-->
 
    
 
    
−
<xacro:servo nombre="antebrazo" />  
+
<servo nombre="antebrazo" />  
 
    
 
    
 
   <joint name="servo_arti2_B" type="fixed">
 
   <joint name="servo_arti2_B" type="fixed">
Line 221: Line 169:
 
   </joint>   
 
   </joint>   
 
    
 
    
−
<xacro:base nombre="antebrazo" />
+
<base nombre="antebrazo" />
 
    
 
    
 
   <joint name="servo_arti2_D" type="fixed">
 
   <joint name="servo_arti2_D" type="fixed">
Line 230: Line 178:
 
   </joint>   
 
   </joint>   
 
    
 
    
−
<xacro:soporte nombre="antebrazo" simetrico="1" lado="I" />   
+
<soporte nombre="antebrazo" simetrico="1" lado="I" />   
 
    
 
    
 
   <joint name="servo_arti2_I" type="fixed">
 
   <joint name="servo_arti2_I" type="fixed">
Line 239: Line 187:
 
   </joint>   
 
   </joint>   
 
    
 
    
−
<xacro:soporte nombre="antebrazo" simetrico="-1" lado="D" />     
+
<soporte nombre="antebrazo" simetrico="-1" lado="D" />     
  
 
   <joint name="arti3" type="revolute">
 
   <joint name="arti3" type="revolute">
Line 246: Line 194:
 
     <origin xyz="0 0 0.104" rpy="0 0 0" />
 
     <origin xyz="0 0 0.104" rpy="0 0 0" />
 
     <axis xyz="1 0 0" />
 
     <axis xyz="1 0 0" />
−
    <limit effort="1000.0" lower="-2.62" upper="2.62" velocity="2"/>  
+
<default_limit />
−
<xacro:default_dynamics />             
+
<default_dynamics />             
 
   </joint>
 
   </joint>
 
    
 
    
Line 269: Line 217:
 
       <origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
 
       <origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
 
       <mass value="0.055"/>
 
       <mass value="0.055"/>
−
  <xacro:default_inertia_servos />
+
  <default_inertia_servos />
 
     </inertial>             
 
     </inertial>             
 
   </link>   
 
   </link>   
Line 299: Line 247:
 
       <origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
 
       <origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>               
 
     </inertial>               
 
   </link>
 
   </link>
Line 329: Line 277:
 
       <origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
 
       <origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>           
 
     </inertial>           
 
   </link>
 
   </link>
Line 359: Line 307:
 
       <origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
 
       <origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>             
 
     </inertial>             
 
   </link>
 
   </link>
Line 391: Line 339:
 
       <origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
 
       <origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
 
       <mass value="0.001"/>
 
       <mass value="0.001"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>           
 
     </inertial>           
 
   </link>
 
   </link>
Line 413: Line 361:
 
     </visual>
 
     </visual>
 
     <collision>
 
     <collision>
−
       <origin rpy="0 0 0" xyz="0.03225 0 0.060"/>
+
       <origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>
 
  <geometry>
 
  <geometry>
−
         <box size="0.002 0.04 0.032"/>
+
         <box size="0.002 0.04 0.037"/>
 
       </geometry>
 
       </geometry>
 
     </collision>
 
     </collision>
 
     <inertial>
 
     <inertial>
−
       <origin rpy="0 0 0" xyz="0.03225 0 0.060"/>
+
       <origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>             
 
     </inertial>             
 
   </link>             
 
   </link>             
Line 430: Line 378:
 
     <origin xyz="-0.00725 0 0.06" rpy="0 0 0" />
 
     <origin xyz="-0.00725 0 0.06" rpy="0 0 0" />
 
     <axis xyz="0 1 0" />
 
     <axis xyz="0 1 0" />
−
    <limit effort="30.0" lower="-2.62" upper="2.62" velocity="2"/>
+
<default_limit />
−
<xacro:default_dynamics />     
+
<default_dynamics />     
 
   </joint>
 
   </joint>
 
    
 
    
Line 455: Line 403:
 
       <origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
 
       <origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
 
       <mass value="0.001"/>
 
       <mass value="0.001"/>
−
    <xacro:default_inertia />
+
    <default_inertia />
 
     </inertial>           
 
     </inertial>           
 
   </link>   
 
   </link>   
Line 485: Line 433:
 
       <origin rpy="0 0 0" xyz="-0.025 0 0"/>
 
       <origin rpy="0 0 0" xyz="-0.025 0 0"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>           
 
     </inertial>           
 
   </link>
 
   </link>
Line 515: Line 463:
 
       <origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
 
       <origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>         
 
     </inertial>         
 
   </link>
 
   </link>
Line 545: Line 493:
 
       <origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
 
       <origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
 
       <mass value="0.0005"/>
 
       <mass value="0.0005"/>
−
  <xacro:default_inertia />
+
  <default_inertia />
 
     </inertial>             
 
     </inertial>             
 
   </link>
 
   </link>
 +
 +
</macro>
 +
   
 +
</robot>
 +
 +
</syntaxhighlight>
 +
 +
El contenido del archivo que contiene los macros, el controlador y transmisiones, que empleamos en el rachivo principal, es el siguiente, que guardaremos en el mismo directorio con el nombre "brazo-macros.xacro":
 +
 +
<syntaxhighlight lang="xml">
 +
 +
<?xml version="1.0"?>
 +
 +
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller">
 +
 +
<macro name="default_inertia">
 +
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
 +
</macro>
 +
 +
<macro name="default_inertia_servos">
 +
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
 +
</macro>
 +
 +
<macro name="default_limit">
 +
    <limit effort="100.0" lower="-2.62" upper="2.62" velocity="3"/>
 +
</macro>
 +
 +
<macro name="default_dynamics">
 +
      <dynamics fricction="0" damping="0" />
 +
</macro>
 +
 +
<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"/>
 +
  <default_inertia_servos />
 +
    </inertial>           
 +
  </link>
 +
</macro>
 +
 +
<macro name="base" params="nombre">   
 +
  <link name="${nombre}_link_B">   
 +
<visual>
 +
      <geometry>
 +
        <box size="0.04 0.032 0.020"/>
 +
      </geometry>
 +
      <origin rpy="0 0 0" xyz="0 0 0.0555"/>
 +
      <material name="white">
 +
<color rgba="1 1 1 1"/>
 +
      </material>
 +
    </visual>
 +
    <collision>
 +
      <origin rpy="0 0 0" xyz="0 0 0.0555"/>
 +
  <geometry>
 +
        <box size="0.04 0.032 0.02"/>
 +
      </geometry>
 +
    </collision>
 +
    <inertial>
 +
      <origin rpy="0 0 0" xyz="0 0 0.0555"/>
 +
      <mass value="0.001"/>
 +
  <default_inertia />
 +
    </inertial>         
 +
  </link>
 +
</macro>
 +
 +
<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"/>
 +
  <default_inertia />
 +
    </inertial>         
 +
  </link>
 +
</macro>
  
 
</robot>
 
</robot>
 +
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Análisis del modelo===
+
Ahora solo falta un archivo desde el que cargaremos el principal donde indicaremos el nombre del ''link'' al que se une  y el origen. Guardaremos un archivo con el nombre "brazo.urdf.xacro" con el siguiente contenido:
 +
 
 +
<syntaxhighlight lang="xml">
 +
 
 +
<?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">
 +
 +
<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
 +
 +
<link name="world" />
 +
 +
<brazo parent="world">
 +
<origin rpy="0 0 0" xyz="0 0 0"/>
 +
</brazo>
 +
 
 +
</robot>
 +
 
 +
</syntaxhighlight>
 +
 
 +
==Análisis del modelo==
  
 
Como estamos empleando [http://wiki.ros.org/xacro xacro] para simplificar y hacer más editable el código necesitamos hacer la conversión a [http://wiki.ros.org/urdf/Tutorials URDF] para obtener el archivo procesado, ejecutando en un terminal la siguiente secuencia de comandos:
 
Como estamos empleando [http://wiki.ros.org/xacro xacro] para simplificar y hacer más editable el código necesitamos hacer la conversión a [http://wiki.ros.org/urdf/Tutorials URDF] para obtener el archivo procesado, ejecutando en un terminal la siguiente secuencia de comandos:
Line 602: Line 686:
 
[[file:modelo_brazo_MYRAbot_arbol_relaciones.jpg|thumb|500px|center|Árbol de relaciones modelo URBF para el brazo del MYRAbot]]
 
[[file:modelo_brazo_MYRAbot_arbol_relaciones.jpg|thumb|500px|center|Árbol de relaciones modelo URBF para el brazo del MYRAbot]]
  
−
==Prueba visual del modelo==
+
=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 [http://wiki.ros.org/robot_state_publisher robot_state_publisher] y ejecutar el [http://wiki.ros.org/joint_state_publisher joint_state_publisher] con el interfaz de publicación del estado de las uniones móviles, y el [http://wiki.ros.org/rviz rviz] para la visualización. Crearemos el archivo "brazo_rviz.launch" en el directorio "launch" del ''package'' que hemos creado, con el siguiente contenido:
 
Para poder visualizar el modelo y comprobar el correcto funcionamiento de las uniones ''joints'' vamos a crear un ''launcher'' para cargarlo en el [http://wiki.ros.org/robot_state_publisher robot_state_publisher] y ejecutar el [http://wiki.ros.org/joint_state_publisher joint_state_publisher] con el interfaz de publicación del estado de las uniones móviles, y el [http://wiki.ros.org/rviz rviz] para la visualización. Crearemos el archivo "brazo_rviz.launch" en el directorio "launch" del ''package'' que hemos creado, con el siguiente contenido:
Line 628: Line 712:
 
Si todo va bien, se ejecutará [http://wiki.ros.org/rviz rviz] y el interfaz del [http://wiki.ros.org/joint_state_publisher 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 [http://wiki.ros.org/rviz rviz]. Para que se muestre correctamente nuestro modelo en [http://wiki.ros.org/rviz 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:
 
Si todo va bien, se ejecutará [http://wiki.ros.org/rviz rviz] y el interfaz del [http://wiki.ros.org/joint_state_publisher 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 [http://wiki.ros.org/rviz rviz]. Para que se muestre correctamente nuestro modelo en [http://wiki.ros.org/rviz 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:
  
−
[[file:modelo_brazo_MYRAbot_rviz.png|thumb|500px|center|Modelo cargado en rviz e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]
+
[[file:modelo_brazo_MYRAbot_rviz.png|thumb|500px|center|Modelo cargado en [http://wiki.ros.org/rviz rviz] e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]
  
−
==Control de uniones para simulación==
+
=Control de uniones para simulación=
  
 
Una vez creado el modelo [http://wiki.ros.org/urdf/Tutorials 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 [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A].
 
Una vez creado el modelo [http://wiki.ros.org/urdf/Tutorials 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 [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] los servomotores [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A].
  
−
===Pasos previos===
+
==Pasos previos==
  
−
Debemos comenzar añadiendo los ''packages'' donde se encuentran los controladores que vamos a emplear. Se trata de los controladores del [http://wiki.ros.org/Robots/PR2 PR2] por lo que necesitamos instalar los ''stacks'' [http://wiki.ros.org/pr2_simulator?distro=electric pr2_simulator] y [http://wiki.ros.org/pr2_controllers pr2_controllers]. Para ello tenemos que ejecutar la siguiente secuencia de comandos en un terminal:
+
Debemos comenzar añadiendo los ''packages'' donde se encuentran los controladores que vamos a emplear. Se trata de los controladores del [http://wiki.ros.org/Robots/PR2 PR2] por lo que necesitamos instalar los ''stacks'' [http://wiki.ros.org/pr2_simulator?distro=electric pr2_simulator], [http://wiki.ros.org/pr2_mechanism pr2_mechanism] y [http://wiki.ros.org/pr2_controllers pr2_controllers]. Para ello tenemos que ejecutar la siguiente secuencia de comandos en un terminal:
  
 
<syntaxhighlight enclose="div">
 
<syntaxhighlight enclose="div">
 
sudo atp-get update
 
sudo atp-get update
−
sudo apt-get install ros-VERSIÓN_ROS-pr2-simulator ros-VERSIÓN_ROS-pr2-controllers
+
sudo apt-get install ros-VERSIÓN_ROS-pr2-simulator ros-VERSIÓN_ROS-pr2-mechanism ros-VERSIÓN_ROS-pr2-controllers
 +
</syntaxhighlight>
 +
 
 +
==Añadir controladores a uniones móviles==
 +
 
 +
===Modificación del URDF===
 +
 
 +
En el [http://wiki.ros.org/urdf/Tutorials 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 [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] debemos asignar a cada elemento un material [http://wiki.ros.org/simulator_gazebo?distro=electric 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 [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo], que añadiremos al final de los ficheros de nuestro [http://wiki.ros.org/urdf/Tutorials URDF] antes de la etiqueta "</robot>", en el archivo "brazo-macros.xacro", y "</macro>", el el archivo "brazo.xacro":
 +
 
 +
* En el archivo brazo-macros.xacro:
 +
 
 +
<syntaxhighlight lang="xml">
 +
 
 +
<macro name="gazebo_propiedades_link" params="nombre material">
 +
  <gazebo reference="${nombre}">
 +
<mu1>0.5</mu1>
 +
<mu2>0.5</mu2>
 +
<material>Gazebo/${material}</material>
 +
    <selfCollide>true</selfCollide>
 +
    <turnGravityOff>false</turnGravityOff>
 +
  </gazebo>
 +
</macro>
 +
 
 +
</syntaxhighlight>
 +
 
 +
* En el archivo brazo.xacro:
 +
 
 +
<syntaxhighlight lang="xml">
 +
 
 +
<gazebo_propiedades_link nombre="base_brazo_link" material="Black" />
 +
 
 +
<gazebo_propiedades_link nombre="hombro_link" material="Black" />
 +
 
 +
<gazebo_propiedades_link nombre="brazo_link" material="Black" /> 
 +
 
 +
<gazebo_propiedades_link nombre="antebrazo_link" material="Black" />
 +
 
 +
<gazebo_propiedades_link nombre="muneca_link" material="Black" />
 +
 
 +
<gazebo_propiedades_link nombre="brazo_link_B" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="brazo_link_SI" material="White" />
 +
 
 +
<gazebo_propiedades_link nombre="brazo_link_SD" material="White" />   
 +
 
 +
<gazebo_propiedades_link nombre="antebrazo_link_B" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="antebrazo_link_SI" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="antebrazo_link_SD" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="muneca_link_B" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="muneca_link_SI" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="muneca_link_SD" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="muneca_link_P" material="White" />
 +
 
 +
<gazebo_propiedades_link nombre="muneca_link_PS" material="White" />   
 +
 
 +
<gazebo_propiedades_link nombre="pinza_link" material="White" /> 
 +
 +
<gazebo_propiedades_link nombre="pinza_link_B" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="pinza_link_SI" material="White" /> 
 +
 
 +
<gazebo_propiedades_link nombre="pinza_link_SD" material="White" />
 +
 
 +
</syntaxhighlight>
 +
 
 +
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 los ficheros de nuestro [http://wiki.ros.org/urdf/Tutorials URDF] antes de la etiqueta "</robot>", en el archivo "brazo-macros.xacro", y "</macro>", el el archivo "brazo.xacro":
 +
 
 +
* En el archivo brazo-macros.xacro:
 +
 
 +
<syntaxhighlight lang="xml">
 +
 
 +
<macro name="gazebo_propiedades_joint" params="nombre">
 +
<gazebo reference="${nombre}">
 +
    <erp>0.1</erp>
 +
    <stopKd value="1000000.0" />
 +
    <stopKp value="10000000.0" />
 +
    <fudgeFactor value="0.5" />
 +
</gazebo>
 +
</macro>
 +
 
 +
<gazebo> 
 +
    <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
 +
          <alwaysOn>true</alwaysOn>
 +
          <updateRate>1000.0</updateRate>
 +
          <robotNamespace>brazo</robotNamespace>
 +
    </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>
 +
 
 +
</syntaxhighlight>
 +
 
 +
* En el archivo brazo.xacro:
 +
 
 +
<syntaxhighlight lang="xml">
 +
 
 +
<gazebo_propiedades_joint nombre="base" />
 +
 
 +
<gazebo_propiedades_joint nombre="arti1" />
 +
 
 +
<gazebo_propiedades_joint nombre="arti2" />
 +
 
 +
<gazebo_propiedades_joint nombre="arti3" />
 +
 +
<gazebo_propiedades_joint nombre="pinza" />
 +
 
 +
</syntaxhighlight>
 +
 
 +
===Creación del fichero de configuración===
 +
 
 +
Los [http://wiki.ros.org/robot_mechanism_controllers?distro=electric 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 [http://es.wikipedia.org/wiki/Proporcional_integral_derivativo 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:
 +
 
 +
<syntaxhighlight>
 +
 
 +
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.0
 +
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Modificaciones del URDF===
+
==Carga del modelo en gazebo==
 +
 
 +
Para poder mostrar el modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric 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:
 +
 
 +
<syntaxhighlight lang="xml" enclose="div">
 +
 
 +
<launch>
 +
 
 +
  <param name="/use_sim_time" value="true" />
 +
 
 +
  <node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
 +
  <node name="gazebo_gui" pkg="gazebo" type="gui" />
 +
 
 +
<group ns="brazo">
 +
 
 +
  <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_arm_model)/urdf/brazo.urdf.xacro'" />
 +
 
 +
  <node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model brazo" respawn="false" output="screen" />
 +
 
 +
  <rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>
 +
 
 +
  <node name="spawn_controller_brazo" 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" />
 +
 
 +
</group>
 +
 +
</launch>
 +
 
 +
</syntaxhighlight>
 +
 
 +
Para lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:
 +
 
 +
<syntaxhighlight>roslaunch brazo_fer_modelo brazo.gazebo.launch</syntaxhighlight>
 +
 
 +
Si todo va bien podremos ver en la ventana gráfica de [http://wiki.ros.org/simulator_gazebo?distro=electric 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:
 +
 
 +
[[file:modelo_brazo_MYRAbot_gazebo.png|thumb|500px|center|Modelo brazo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric 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:
 +
 
 +
<syntaxhighlight>rostopic echo</syntaxhighlight>
 +
 
 +
Debería aparecernos algo similar a lo que se muestra a continuación, donde el ''topic'' brazo/NOMBRE_CONTROLADOR/command es al que está suscrito el controlador para recibir el valor de consigna y el ''topic'' brazo/NOMBRE_CONTROLADOR/state es en el que publica el estado del controlador:
 +
 
 +
<syntaxhighlight>
 +
/brazo/arti1_pos_controller/command
 +
/brazo/arti1_pos_controller/state
 +
/brazo/arti1_vel_controller/command
 +
/brazo/arti1_vel_controller/state
 +
/brazo/arti2_pos_controller/command
 +
/brazo/arti2_pos_controller/state
 +
/brazo/arti2_vel_controller/command
 +
/brazo/arti2_vel_controller/state
 +
/brazo/arti3_pos_controller/command
 +
/brazo/arti3_pos_controller/state
 +
/brazo/arti3_vel_controller/command
 +
/brazo/arti3_vel_controller/state
 +
/brazo/base_pos_controller/command
 +
/brazo/base_pos_controller/state
 +
/brazo/base_vel_controller/command
 +
/brazo/base_vel_controller/state
 +
/brazo/joint_states
 +
/brazo/mechanism_statistics
 +
/brazo/pinza_pos_controller/command
 +
/brazo/pinza_pos_controller/state
 +
/brazo/pinza_vel_controller/command
 +
/brazo/pinza_vel_controller/state
 +
/clock
 +
/gazebo/link_states
 +
/gazebo/model_states
 +
/gazebo/parameter_descriptions
 +
/gazebo/parameter_updates
 +
/gazebo/set_link_state
 +
/gazebo/set_model_state
 +
/rosout
 +
/rosout_agg
 +
/tf
 +
</syntaxhighlight>
 +
 
 +
==Programa interfaz (gazebo-programas de control)==
 +
 
 +
Al igual que en el [[Control brazo MYRAbot (bioloid+arduino)|brazo real]], que empleabamos una placa [http://www.arduino.cc/ arduino] que contenía el programa de interfaz entre el brazo y [[Control brazo MYRAbot (bioloid+arduino)#Programas de control|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:
 +
 
 +
<syntaxhighlight lang="cpp" enclose="div">
 +
 
 +
  #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>("brazo/base_pos_controller/command", 1); 
 +
  ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_pos_controller/command", 1);
 +
  ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_pos_controller/command", 1);   
 +
  ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_pos_controller/command", 1);
 +
 
 +
  ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("brazo/base_vel_controller/command", 1); 
 +
  ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_vel_controller/command", 1);
 +
  ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_vel_controller/command", 1);   
 +
  ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("brazo/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 && (position.base >= 0 && position.base <= 1023) && (speed.base >= 0 && speed.base <= 1023))
 +
{
 +
base_pos_command.data = ((((position.base * 300)/1023)*PI)/180)-2.62;
 +
base_vel_command.data = speed.base/511;
 +
}
 +
if (torque.arti1 != 0 && (position.arti1 >= 0 && position.arti1 <= 1023) && (speed.arti1 >= 0 && speed.arti1 <= 1023))
 +
{
 +
arti1_pos_command.data = ((((position.arti1*300)/1023)*PI)/180)-2.62;
 +
arti1_vel_command.data = speed.arti1/511;
 +
}
 +
if (torque.arti2 != 0 && (position.arti2 >= 0 && position.arti2 <= 1023) && (speed.arti2 >= 0 && speed.arti2 <= 1023))
 +
{
 +
arti2_pos_command.data = ((((position.arti2*300)/1023)*PI)/180)-2.62;
 +
arti2_vel_command.data = speed.arti2/511;
 +
}
 +
if (torque.arti3 != 0 && (position.arti3 >= 0 && position.arti3 <= 1023) && (speed.arti3 >= 0 && speed.arti3 <= 1023))
 +
{
 +
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>("brazo/pinza_pos_controller/command", 1);
 +
 
 +
  ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("brazo/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 && (position.pinza >= 0 && position.pinza <= 1023) && (speed.pinza >= 0 && speed.pinza <= 1023))
 +
{
 +
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("brazo/joint_states", 1, pe);
 +
     
 +
 +
  ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("brazo/base_pos_controller/command", 1); 
 +
  ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_pos_controller/command", 1);
 +
  ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_pos_controller/command", 1);   
 +
  ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_pos_controller/command", 1);
 +
  ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_pos_controller/command", 1);
 +
   
 +
  ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("brazo/base_vel_controller/command", 1); 
 +
  ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_vel_controller/command", 1);
 +
  ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_vel_controller/command", 1);   
 +
  ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_vel_controller/command", 1); 
 +
  ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_vel_controller/command", 1);
 +
 
 +
  ros::Publisher pose_pub_=n.advertise<brazo_fer::ReadServos>("pose_arm", 1);     
 +
 +
ros::spin(); 
 +
 +
        return 0;
 +
  }
 +
 
 +
</syntaxhighlight>
 +
 
 +
 
 +
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:
 +
 
 +
<syntaxhighlight>rosbuild_add_executable(control_modelo src/control_modelo.cpp)</syntaxhighlight>
 +
 
 +
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:
 +
 
 +
<syntaxhighlight>roscd brazo_fer_modelo
 +
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. Deberemos añadir la siguiente linea al final antes de la etiqueta "</launch>":
 +
 
 +
<syntaxhighlight lang="xml" enclose="div">
 +
 
 +
  <node name="control_modelo" pkg="brazo_fer_modelo" type="control_modelo" />
 +
 
 +
</syntaxhighlight>
 +
 
 +
==Primer programa de control (Ven aquí)==
 +
 
 +
El  [[Control brazo MYRAbot (bioloid+arduino)#Tercer programa (Ven aquí)|programa de control]] es el mismo que se ha desarrollado para el brazo real. Para su simulación ejecutaremos primero el ''launcher'' que inicia el simulador, carga el modelo y los controladores, y el programa interfaz, ejecutando en un terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>roslaunch brazo_fer_modelo brazo_gazebo.launch</syntaxhighlight>
 +
 
 +
Una vez se haya iniciado el [http://wiki.ros.org/simulator_gazebo?distro=electric simulador gazebo] podremos iniciar el programa de control ejecutando en otro terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>
 +
 
 +
Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 100, y: 0, z: 180}' --once</syntaxhighlight>
 +
 
 +
El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde su posición inicial:
 +
 
 +
<center><videoflash>S9kUKYysm0M</videoflash></center>
 +
 
 +
<center>[http://www.youtube.com/watch?v=S9kUKYysm0M Ver vídeo en YouTube]</center>
 +
 
 +
==Segundo programa de control (Agarra la lata)==
 +
 
 +
Lo primero debemos crear la lata que va a coger el brazo. Crearemos un archivo [http://wiki.ros.org/urdf/Tutorials URDF] llamado "lata.urdf", que colocaremos en nuestro ''package'', con el siguiente contenido:
 +
 
 +
<syntaxhighlight lang="xml">
 +
 
 +
<?xml version="1.0"?>
 +
 
 +
<robot name="lata">
 +
 
 +
<link name="world" />
 +
 
 +
<joint name="fixed" type="floating">
 +
  <parent link="world"/>
 +
  <child link="lata_link"/>
 +
  <origin xyz="0 0 0" rpy="0 0 0" />
 +
</joint>
 +
 
 +
<link name="lata_link">
 +
  <visual>
 +
    <geometry>
 +
      <cylinder radius="0.03" length="0.125" />
 +
    </geometry>
 +
    <origin rpy="0 0 0" xyz="0 0 0.0625" />
 +
    <material name="red">
 +
      <color rgba="1 0 0 1" />
 +
    </material>
 +
  </visual>
 +
  <collision>
 +
    <origin rpy="0 0 0" xyz="0 0 0.0625" />
 +
    <geometry>
 +
      <cylinder radius="0.03" length="0.125" />
 +
    </geometry>
 +
  </collision>
 +
  <inertial>
 +
    <origin rpy="0 0 0" xyz="0 0 0.0625" />
 +
    <mass value="0.01" />
 +
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/> 
 +
  </inertial>
 +
</link>
 +
 
 +
<gazebo reference="lata_link">
 +
  <material>Gazebo/Red</material>
 +
  <mu1>0.9</mu1>
 +
  <mu2>0.9</mu2>
 +
  <selfCollide>true</selfCollide>
 +
  <turnGravityOff>false</turnGravityOff> 
 +
</gazebo>
 +
 
 +
</robot>
 +
 
 +
</syntaxhighlight>
 +
 
 +
También deberemos añadir la siguiente línea al archvo "brazo_gazebo.launch", que carga la lata en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] en la posición xyz = (-0.15 0.21 0):
 +
 
 +
<syntaxhighlight lang="xml" enclose="div"><node name="spawn_lata" pkg="gazebo" type="spawn_model" args="-file $(find brazo_fer_modelo)/lata.urdf -urdf -x -0.15 -y 0.21 -model lata" respawn="false" output="screen" /></syntaxhighlight>
 +
 
 +
El  [[Control brazo MYRAbot (bioloid+arduino)#Cuarto programa (Agarra la botella)|programa de control]] es el mismo que se ha desarrollado para el brazo real. Para su simulación ejecutaremos primero el ''launcher'' que inicia el simulador, carga el modelo y los controladores,la lata y el programa interfaz, ejecutando en un terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>roslaunch brazo_fer_modelo brazo_gazebo.launch</syntaxhighlight>
 +
 
 +
Una vez se haya iniciado el [http://wiki.ros.org/simulator_gazebo?distro=electric simulador gazebo] podremos iniciar el programa de control ejecutando en otro terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
 +
 
 +
Para poder publicar las coordenadas del punto donde se encuentra la lata, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando:
 +
 
 +
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 145, y: 0, z: 180}' --once</syntaxhighlight>
 +
 
 +
* '''nota''': El sismtema de coordenadas del simulador no coincide con el tomado para el brazo, por lo que el punto no es el mismo cuando indicamos la posición del objeto en el mundo y cuando indicamos la posición del objeto al brazo, además hemos indicado un punto alejado del centro del objeto para evitar la colisión.
 +
 
 +
El brazo se situará en la posición de partida y al indicarle el punto donde se encuentra el objeto se desclazará hasta sus proximidades y se situará para para agarrarlo, cuando lo tenga sujeto se lo llevará cerca de él. En el siguiente vídeo se puede ver como el brazo recoge la lata partiendo desde su posición inicial:
 +
 
 +
<center><videoflash>16aR8XXHnmw</videoflash></center>
 +
 
 +
<center>[http://www.youtube.com/watch?v=16aR8XXHnmw Ver vídeo en YouTube]</center>
 +
<!--http://youtu.be/16aR8XXHnmw-->
 +
 
 +
 
 +
----
 +
{| style="border: solid 0px white; width: 100%"
 +
!Artículos realcionados
 +
----
 +
!Otros artículos
 +
----
 +
|-
 +
| valign="top" width="50%" |
 +
 
 +
[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
 +
[[Detección y cálculo de posición de objetos (cámara web)]]<br/>
 +
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<br/>
 +
[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
 +
[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
 +
 
 +
| valign="top" |
 +
 
 +
[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
 +
[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
 +
[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
 +
[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
 +
----
 +
[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
 +
 
 +
|}
 +
----
 +
 
  
 
[[Mobile manipulation | < volver a principal]]
 
[[Mobile manipulation | < volver a principal]]

Latest revision as of 11:03, 29 September 2014

< volver a principal


Artículos realcionados
Otros artículos

Control brazo MYRAbot (bioloid+arduino)
Detección y cálculo de posición de objetos (cámara web)
Modelo para simulación MYRAbot (urdf+gazebo)
Integración de MYRAbot en moveIt! (gazebo+moveIt!)
Órdenes y confirmación mediante voz (sphinx+festival)
Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

Control brazo y base CeRVaNTeS (maxon+epos2)
Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)
Modelo para simulación CeRVaNTeS (urdf+gazebo)
Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)
Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

Programas de control para el robot Turtlebot sobre ROS


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, lineales o flotantes.

Comenzaremos creando el package "brazo_fer_modelo" que va a contener el modelo de nuestro brazo, con las siguientes dependencias: urdf brazo_fer std_msgs sensor_msgs tf roscpp

El modelo planteado para nuestro brazo se compone de tres archivos, uno que contiene los macros que vamos a utilizar en el archivo principal del modelo y un archivo en el que marcamos el link al que está unido el modelo. El contenido del arcivo principal del modelo es el siguiente, que guardaremos en el directorio "urdf" del package creado en un archivo llamado "brazo.xacro": 

<?xml version="1.0"?>

<robot>

	<include filename="$(find brazo_fer_modelo)/urdf/brazo-macros.xacro" />

<macro name="brazo" params="parent *origin">

  <joint name="fixed" type="fixed">
    <parent link="${parent}"/>
    <child link="base_brazo_link"/>
    <insert_block name="origin" />
    <axis xyz="0 0 1" />  
  </joint>
    
  <link name="base_brazo_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"/>
	  <default_inertia_servos />
    </inertial>       
  </link>

  <joint name="base" type="revolute">
    <parent link="base_brazo_link"/>
    <child link="hombro_link"/>
    <origin xyz="0 0 0.04" rpy="0 0 0" />
    <axis xyz="0 0 1" />
	<default_limit />
	<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.025"/>
      <material name="black" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.025"/>		
	  <geometry>
        <box size="0.04 0.032 0.05"/>
      </geometry>       
    </collision> 
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.025"/>			
      <mass value="0.055"/>
	  <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" />
	<default_limit />
	<default_dynamics />        
  </joint>
  
  <!--brazo-->

<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>  

<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>  

<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>  
  
<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" />
	<default_limit />
	<default_dynamics />            
  </joint>
  
  <!--antebrazo-->
  
<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>  
  
<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>  
  
<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>  
  
<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" />
	<default_limit />
	<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"/>
	  <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"/>
	  <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"/>
	  <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"/>
	  <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"/>
	  <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.0575"/>	
	  <geometry>
        <box size="0.002 0.04 0.037"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>			
      <mass value="0.0005"/>
	  <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" />
	<default_limit />
	<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"/>
 	  <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"/>
	  <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"/>
	  <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"/>
	  <default_inertia />
    </inertial>            
  </link>

</macro>
     
</robot>

El contenido del archivo que contiene los macros, el controlador y transmisiones, que empleamos en el rachivo principal, es el siguiente, que guardaremos en el mismo directorio con el nombre "brazo-macros.xacro": 

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller">

<macro name="default_inertia">
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</macro>

<macro name="default_inertia_servos">
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</macro>

<macro name="default_limit">
    <limit effort="100.0" lower="-2.62" upper="2.62" velocity="3"/>
</macro>

<macro name="default_dynamics">
      <dynamics fricction="0" damping="0" />
</macro>

<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"/>
	  <default_inertia_servos />
    </inertial>             
  </link>
</macro>

<macro name="base" params="nombre">    
  <link name="${nombre}_link_B">    
	<visual>
      <geometry>
        <box size="0.04 0.032 0.020"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.0555"/>
      <material name="white">
		<color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0555"/>		
	  <geometry>
        <box size="0.04 0.032 0.02"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.0555"/>			
      <mass value="0.001"/>
	  <default_inertia />
    </inertial>           
  </link>
</macro> 

<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"/>
	  <default_inertia />
    </inertial>           
  </link>
</macro>

</robot>

Ahora solo falta un archivo desde el que cargaremos el principal donde indicaremos el nombre del link al que se une y el origen. Guardaremos un archivo con el nombre "brazo.urdf.xacro" con el siguiente contenido: 

<?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">
	
	<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
	
	<link name="world" />
	
	<brazo parent="world">
		<origin rpy="0 0 0" xyz="0 0 0"/>
	</brazo>

</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.urdf

Ahora podemos comprobar si el modelo que hemos creado es correcto ejecutando el siguiente comando en el terminal: 

rosrun urdf_parser check_urdf brazo.urdf

Si 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_SD

Podemos 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.urdf

El resultado obtenido será el siguiente:

Árbol de relaciones modelo URBF para el brazo del MYRAbot

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.launch

Si 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-controllers

Añ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 los ficheros de nuestro URDF antes de la etiqueta "</robot>", en el archivo "brazo-macros.xacro", y "</macro>", el el archivo "brazo.xacro":

  • En el archivo brazo-macros.xacro:
<macro name="gazebo_propiedades_link" params="nombre material">
  <gazebo reference="${nombre}">
	<mu1>0.5</mu1>
	<mu2>0.5</mu2>
	<material>Gazebo/${material}</material>
    <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff> 
  </gazebo>
</macro>
  • En el archivo brazo.xacro:
<gazebo_propiedades_link nombre="base_brazo_link" material="Black" />

<gazebo_propiedades_link nombre="hombro_link" material="Black" />
  
<gazebo_propiedades_link nombre="brazo_link" material="Black" />  
  
<gazebo_propiedades_link nombre="antebrazo_link" material="Black" />
   
<gazebo_propiedades_link nombre="muneca_link" material="Black" />
  
<gazebo_propiedades_link nombre="brazo_link_B" material="White" />   
  
<gazebo_propiedades_link nombre="brazo_link_SI" material="White" /> 

<gazebo_propiedades_link nombre="brazo_link_SD" material="White" />     

<gazebo_propiedades_link nombre="antebrazo_link_B" material="White" />   

<gazebo_propiedades_link nombre="antebrazo_link_SI" material="White" />   

<gazebo_propiedades_link nombre="antebrazo_link_SD" material="White" />   
  
<gazebo_propiedades_link nombre="muneca_link_B" material="White" />   

<gazebo_propiedades_link nombre="muneca_link_SI" material="White" />   
  
<gazebo_propiedades_link nombre="muneca_link_SD" material="White" />   

<gazebo_propiedades_link nombre="muneca_link_P" material="White" /> 

<gazebo_propiedades_link nombre="muneca_link_PS" material="White" />     
  
<gazebo_propiedades_link nombre="pinza_link" material="White" />   
 
<gazebo_propiedades_link nombre="pinza_link_B" material="White" />   
  
<gazebo_propiedades_link nombre="pinza_link_SI" material="White" />   
   
<gazebo_propiedades_link nombre="pinza_link_SD" material="White" />

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 los ficheros de nuestro URDF antes de la etiqueta "</robot>", en el archivo "brazo-macros.xacro", y "</macro>", el el archivo "brazo.xacro":

  • En el archivo brazo-macros.xacro:
<macro name="gazebo_propiedades_joint" params="nombre">
 <gazebo reference="${nombre}">
    <erp>0.1</erp>
    <stopKd value="1000000.0" />
    <stopKp value="10000000.0" />
    <fudgeFactor value="0.5" />
 </gazebo>
</macro> 

<gazebo>   
     <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>1000.0</updateRate>
          <robotNamespace>brazo</robotNamespace>
     </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>
  • En el archivo brazo.xacro:
<gazebo_propiedades_joint nombre="base" />
  
<gazebo_propiedades_joint nombre="arti1" />

<gazebo_propiedades_joint nombre="arti2" />

<gazebo_propiedades_joint nombre="arti3" />
 
<gazebo_propiedades_joint nombre="pinza" />

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.0

Carga 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>

  <param name="/use_sim_time" value="true" />

  <node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
  <node name="gazebo_gui" pkg="gazebo" type="gui" />

<group ns="brazo">

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_arm_model)/urdf/brazo.urdf.xacro'" />

  <node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model brazo" respawn="false" output="screen" />

  <rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>

  <node name="spawn_controller_brazo" 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" />
  
</group>
 
</launch>

Para lanzar el launcher ejecutaremos el siguiente comando en un terminal: 

roslaunch brazo_fer_modelo brazo.gazebo.launch

Si todo va bien podremos ver en la ventana grá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 echo

Debería aparecernos algo similar a lo que se muestra a continuación, donde el topic brazo/NOMBRE_CONTROLADOR/command es al que está suscrito el controlador para recibir el valor de consigna y el topic brazo/NOMBRE_CONTROLADOR/state es en el que publica el estado del controlador: 

/brazo/arti1_pos_controller/command
/brazo/arti1_pos_controller/state
/brazo/arti1_vel_controller/command
/brazo/arti1_vel_controller/state
/brazo/arti2_pos_controller/command
/brazo/arti2_pos_controller/state
/brazo/arti2_vel_controller/command
/brazo/arti2_vel_controller/state
/brazo/arti3_pos_controller/command
/brazo/arti3_pos_controller/state
/brazo/arti3_vel_controller/command
/brazo/arti3_vel_controller/state
/brazo/base_pos_controller/command
/brazo/base_pos_controller/state
/brazo/base_vel_controller/command
/brazo/base_vel_controller/state
/brazo/joint_states
/brazo/mechanism_statistics
/brazo/pinza_pos_controller/command
/brazo/pinza_pos_controller/state
/brazo/pinza_vel_controller/command
/brazo/pinza_vel_controller/state
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/rosout
/rosout_agg
/tf

Programa 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>("brazo/base_pos_controller/command", 1);   
  	ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_pos_controller/command", 1); 
   	ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_pos_controller/command", 1);  	   
   	ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_pos_controller/command", 1);
   	
   	ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("brazo/base_vel_controller/command", 1);   
  	ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_vel_controller/command", 1); 
   	ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_vel_controller/command", 1);  	   
   	ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("brazo/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 && (position.base >= 0 && position.base <= 1023) && (speed.base >= 0 && speed.base <= 1023))
	{		
		base_pos_command.data = ((((position.base * 300)/1023)*PI)/180)-2.62;
		base_vel_command.data = speed.base/511;
	}
	if (torque.arti1 != 0 && (position.arti1 >= 0 && position.arti1 <= 1023) && (speed.arti1 >= 0 && speed.arti1 <= 1023))
	{
		arti1_pos_command.data = ((((position.arti1*300)/1023)*PI)/180)-2.62;
		arti1_vel_command.data = speed.arti1/511;		
	}
	if (torque.arti2 != 0 && (position.arti2 >= 0 && position.arti2 <= 1023) && (speed.arti2 >= 0 && speed.arti2 <= 1023))
	{
		arti2_pos_command.data = ((((position.arti2*300)/1023)*PI)/180)-2.62;
		arti2_vel_command.data = speed.arti2/511;		
	}
	if (torque.arti3 != 0 && (position.arti3 >= 0 && position.arti3 <= 1023) && (speed.arti3 >= 0 && speed.arti3 <= 1023))
	{
		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>("brazo/pinza_pos_controller/command", 1);
 	   
   	ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("brazo/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 && (position.pinza >= 0 && position.pinza <= 1023) && (speed.pinza >= 0 && speed.pinza <= 1023))
	{
		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("brazo/joint_states", 1, pe);
  	  	   	 
 
   	ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("brazo/base_pos_controller/command", 1);   
  	ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_pos_controller/command", 1); 
   	ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_pos_controller/command", 1);  	   
   	ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_pos_controller/command", 1);
   	ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_pos_controller/command", 1);
 	    	
   	ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("brazo/base_vel_controller/command", 1);   
  	ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_vel_controller/command", 1); 
   	ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_vel_controller/command", 1);  	   
   	ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_vel_controller/command", 1);  
   	ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("brazo/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
make

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. Deberemos añadir la siguiente linea al final antes de la etiqueta "</launch>": 

  <node name="control_modelo" pkg="brazo_fer_modelo" type="control_modelo" />

Primer programa de control (Ven aquí)

El programa de control es el mismo que se ha desarrollado para el brazo real. Para su simulación ejecutaremos primero el launcher que inicia el simulador, carga el modelo y los controladores, y el programa interfaz, ejecutando en un terminal el siguiente comando: 

roslaunch brazo_fer_modelo brazo_gazebo.launch

Una vez se haya iniciado el simulador gazebo podremos iniciar el programa de control ejecutando en otro terminal el siguiente comando: 

rosrun brazo_fer control_v01

Para poder publicar las coordenadas del punto al que queremos que se desplace el brazo, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando: 

rostopic pub point geometry_msgs/Point '{x: 100, y: 0, z: 180}' --once

El brazo comenzará a desplazarse y se detendrá al llegar al punto que le hemos indicado. En el siguiente vídeo se puede ver como el brazo se posiciona en el punto que le indicamos desde su posición inicial:

<videoflash>S9kUKYysm0M</videoflash>
Ver vídeo en YouTube

Segundo programa de control (Agarra la lata)

Lo primero debemos crear la lata que va a coger el brazo. Crearemos un archivo URDF llamado "lata.urdf", que colocaremos en nuestro package, con el siguiente contenido: 

<?xml version="1.0"?>

<robot name="lata">

<link name="world" />

<joint name="fixed" type="floating">
  <parent link="world"/>
  <child link="lata_link"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
  <visual>
    <geometry>
      <cylinder radius="0.03" length="0.125" />
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0.0625" />
    <material name="red">
      <color rgba="1 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 0.0625" />
    <geometry>
      <cylinder radius="0.03" length="0.125" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 0.0625" />
    <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>  
  </inertial>
</link>

<gazebo reference="lata_link">
  <material>Gazebo/Red</material>
  <mu1>0.9</mu1>
  <mu2>0.9</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

</robot>

También deberemos añadir la siguiente línea al archvo "brazo_gazebo.launch", que carga la lata en gazebo en la posición xyz = (-0.15 0.21 0): 

<node name="spawn_lata" pkg="gazebo" type="spawn_model" args="-file $(find brazo_fer_modelo)/lata.urdf -urdf -x -0.15 -y 0.21 -model lata" respawn="false" output="screen" />

El programa de control es el mismo que se ha desarrollado para el brazo real. Para su simulación ejecutaremos primero el launcher que inicia el simulador, carga el modelo y los controladores,la lata y el programa interfaz, ejecutando en un terminal el siguiente comando: 

roslaunch brazo_fer_modelo brazo_gazebo.launch

Una vez se haya iniciado el simulador gazebo podremos iniciar el programa de control ejecutando en otro terminal el siguiente comando: 

rosrun brazo_fer control_v02

Para poder publicar las coordenadas del punto donde se encuentra la lata, debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando: 

rostopic pub point geometry_msgs/Point '{x: 145, y: 0, z: 180}' --once
  • nota: El sismtema de coordenadas del simulador no coincide con el tomado para el brazo, por lo que el punto no es el mismo cuando indicamos la posición del objeto en el mundo y cuando indicamos la posición del objeto al brazo, además hemos indicado un punto alejado del centro del objeto para evitar la colisión.

El brazo se situará en la posición de partida y al indicarle el punto donde se encuentra el objeto se desclazará hasta sus proximidades y se situará para para agarrarlo, cuando lo tenga sujeto se lo llevará cerca de él. En el siguiente vídeo se puede ver como el brazo recoge la lata partiendo desde su posición inicial:

<videoflash>16aR8XXHnmw</videoflash>
Ver vídeo en YouTube


Artículos realcionados
Otros artículos

Control brazo MYRAbot (bioloid+arduino)
Detección y cálculo de posición de objetos (cámara web)
Modelo para simulación MYRAbot (urdf+gazebo)
Integración de MYRAbot en moveIt! (gazebo+moveIt!)
Órdenes y confirmación mediante voz (sphinx+festival)
Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

Control brazo y base CeRVaNTeS (maxon+epos2)
Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)
Modelo para simulación CeRVaNTeS (urdf+gazebo)
Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)
Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

Programas de control para el robot Turtlebot sobre ROS


< volver a principal

Retrieved from "https://robotica.unileon.es/index.php?title=Modelo_para_simulación_brazo_MYRAbot_(urdf%2bgazebo)&oldid=4209"