Difference between revisions of "MYRAbot's arm model for simulation (urdf+gazebo)"

From robotica.unileon.es
Jump to: navigation, search
 
(56 intermediate revisions by 2 users not shown)
Line 1: Line 1:
 
[[Mobile manipulation | < go back to main]]
 
[[Mobile manipulation | < go back to main]]
  
−
==Creation of URDF model==
 
  
−
<!----
+
----
−
==Creación del modelo URDF==
+
{| style="border: solid 0px white; width: 100%"
 +
!Related articles
 +
----
 +
!Other articles
 +
----
 +
|-
 +
| valign="top" width="50%" |
  
−
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 o lineales.
+
[[MYRAbot's arm control (bioloid+arduino)]]<br/>
 +
[[Objects recognition and position calculation (webcam)]]<br/>
 +
[[MYRAbot model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Voice control (sphinx+festival)]]<br/>
 +
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
  
−
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 pependencias: urdf [[Control brazo MYRAbot (bioloid+arduino)|brazo_fer]] std_msgs sensor_msgs tf roscpp
+
| valign="top" |
  
−
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":
+
[[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/>
 +
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/>
 +
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 +
 
 +
|}
 +
----
 +
 
 +
 
 +
=Creation of URDF model=
 +
 
 +
[http://wiki.ros.org/urdf/Tutorials URDF] (Unified Robot Description Format) is the format used to define the model of the [[MYRAbot's arm control (bioloid+arduino)|arm]] ([http://es.wikipedia.org/wiki/Extensible_Markup_Language xml]) in order to simulate it in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. It consists of a tree of geometrical elements (''links'') plugged through joints(''joints''). This joints can be fixed or mobile, the mobile joints can be rotating, linear or floating.
 +
 
 +
First, we will create a new ''package'' with the necessary dependences for our programs (urdf [[Control brazo MYRAbot (bioloid+arduino)|brazo_fer]] std_msgs sensor_msgs tf roscpp). We will execute the next commands in a terminal:
 +
 
 +
<syntaxhighlight>
 +
cd ~/ros_workspace
 +
roscreate-pkg brazo_fer_modelo urdf brazo_fer std_msgs sensor_msgs tf roscpp</syntaxhighlight>
 +
 
 +
We have divided the model in three files, main file, ''macros'' file and placement file. For the main file, we will create a file named "brazo.xacro" within the folder "urdf" of the created ''package'' with the content that is shown below:
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
Line 90: Line 120:
 
   </joint>
 
   </joint>
 
    
 
    
−
  <!--brazo-->
+
<!--Brazo-->
  
 
<servo nombre="brazo" />
 
<servo nombre="brazo" />
Line 130: Line 160:
 
   </joint>
 
   </joint>
 
    
 
    
−
  <!--antebrazo-->
+
<!--Antebrazo-->
 
    
 
    
 
<servo nombre="antebrazo" />  
 
<servo nombre="antebrazo" />  
Line 170: Line 200:
 
   </joint>
 
   </joint>
 
    
 
    
−
  <!--muneca-->
+
<!--Muñeca-->
 
    
 
    
 
   <link name="muneca_link">
 
   <link name="muneca_link">
Line 289: Line 319:
 
<axis xyz="1 0 0" />     
 
<axis xyz="1 0 0" />     
 
   </joint>   
 
   </joint>   
−
 
+
 
−
  <!--pinza izquierda fija-->
+
<!--Pinza fija-->
 
    
 
    
 
   <link name="muneca_link_P">     
 
   <link name="muneca_link_P">     
Line 354: Line 384:
 
   </joint>
 
   </joint>
 
    
 
    
−
  <!--pinza-->
+
<!--Pinza móvil-->
 
    
 
    
 
   <link name="pinza_link">     
 
   <link name="pinza_link">     
Line 475: Line 505:
 
</syntaxhighlight>
 
</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":
+
For the ''macros'' file, we will create a file named "brazo-macros.xacro" within the folder "src" of our ''package'' with the content that is shown below:
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
Line 576: Line 606:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
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:
+
For the placement file, we will create a file named "brazo.urdf.xacro" within the folder "src" of our ''package'' with the content that is shown below:
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
Line 608: Line 638:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Análisis del modelo===
+
==Model analysis==
  
−
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:
+
As we have used [http://wiki.ros.org/xacro xacro] in order to simplify and to make the code more editable, we will execute the next commands in a terminal in order to obtain the [http://wiki.ros.org/urdf/Tutorials URDF] file processed:
  
 
<syntaxhighlight>roscd brazo_fer_modelo
 
<syntaxhighlight>roscd brazo_fer_modelo
Line 616: Line 646:
 
rosrun xacro xacro.py brazo.urdf.xacro > brazo.urdf</syntaxhighlight>
 
rosrun xacro xacro.py brazo.urdf.xacro > brazo.urdf</syntaxhighlight>
  
−
Ahora podemos comprobar si el modelo que hemos creado es correcto ejecutando el siguiente comando en el terminal:
+
We will execute the next command in a terminal to check the model:
  
 
<syntaxhighlight>rosrun urdf_parser check_urdf brazo.urdf</syntaxhighlight>
 
<syntaxhighlight>rosrun urdf_parser check_urdf brazo.urdf</syntaxhighlight>
  
−
Si todo va bien deberiamos obtener lo siguiente:
+
We must obtain something similar to the following:
  
 
<syntaxhighlight>
 
<syntaxhighlight>
Line 650: Line 680:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
Podemos ver el árbol de relaciones de forma gráfica generando un archivo pdf, para lo que ejecutaremos en el terminal el siguiente comando:
+
We will execute the next command in a terminal in order to see the graphic tree generated in a .pdf file:
  
 
<syntaxhighlight>rosrun urdf_parser urdf_to_graphiz brazo.urdf</syntaxhighlight>
 
<syntaxhighlight>rosrun urdf_parser urdf_to_graphiz brazo.urdf</syntaxhighlight>
  
−
El resultado obtenido será el siguiente:
+
We must obtain something similar to the following:
  
 
[[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==
+
=Visual test of the model=
  
−
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:
+
We will create a file named "brazo_rviz.launch" within the folder "launch" of our ''package'' with the content that is shown below in order to display the model and to check the ''joints'':
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
Line 678: Line 708:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
Para iniciar el ''launcher'' debemos ejecutar en un terminal el siguiente comando:
+
We will execute the next command in a terminal in order to start the ''launcher'':
  
 
<syntaxhighlight>roslaunch brazo_fer_modelo brazo_rviz.launch</syntaxhighlight>
 
<syntaxhighlight>roslaunch brazo_fer_modelo brazo_rviz.launch</syntaxhighlight>
  
−
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:
+
The ''launcher'' starts [http://wiki.ros.org/rviz rviz] and the [http://en.wikipedia.org/wiki/Graphical_user_interface GUI] of [http://wiki.ros.org/joint_state_publisher joint_state_publisher]. We have to set in "Global Options" the field "Fixed Frame" to "/world" ''link''. We can change the position of the joints using the sliders of the [http://en.wikipedia.org/wiki/Graphical_user_interface GUI] of [http://wiki.ros.org/joint_state_publisher joint_state_publisher]. We must obtain something similar to the following:  
  
−
[[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]]]
+
[[file:modelo_brazo_MYRAbot_rviz.png|thumb|500px|center|Loaded model in [http://wiki.ros.org/rviz rviz] and the [http://en.wikipedia.org/wiki/Graphical_user_interface GUI] of [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]
  
−
==Control de uniones para simulación==
+
=Control of the joints for simulation=
  
−
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].
+
We have to add a controller to each mobile joint in order to simulate the servo motors [http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm Dynamixel AX-12A] in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]
  
−
===Pasos previos===
+
==Previous steps==
  
−
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:
+
We will execute the next commands in a terminal in order to install the control ''stacks'' of the [http://wiki.ros.org/Robots/PR2 PR2] robot ([http://wiki.ros.org/pr2_simulator?distro=electric pr2_simulator], [http://wiki.ros.org/pr2_mechanism pr2_mechanism] and [http://wiki.ros.org/pr2_controllers pr2_controllers):
  
 
<syntaxhighlight enclose="div">
 
<syntaxhighlight enclose="div">
Line 699: Line 729:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Añadir controladores a uniones móviles===
+
==To add controllers to mobile joints==
  
−
====Modificación del URDF====
+
===Modification of the 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":
+
We have to set the materials, properties, transmissions and controllers for our [http://wiki.ros.org/urdf/Tutorials URDF] model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo].  
  
−
* En el archivo brazo-macros.xacro:
+
We will add the next code lines before the tag "</robot>" within the file "brazo-macros.xacro":
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
Line 718: Line 748:
 
   </gazebo>
 
   </gazebo>
 
</macro>
 
</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">
 
<macro name="gazebo_propiedades_joint" params="nombre">
Line 786: Line 762:
 
           <alwaysOn>true</alwaysOn>
 
           <alwaysOn>true</alwaysOn>
 
           <updateRate>1000.0</updateRate>
 
           <updateRate>1000.0</updateRate>
 +
          <robotNamespace>brazo</robotNamespace>
 
     </controller:gazebo_ros_controller_manager>
 
     </controller:gazebo_ros_controller_manager>
 
</gazebo>
 
</gazebo>
Line 826: Line 803:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
* En el archivo brazo.xacro:
+
We will add the next code lines before the tag "</macro>" within the file "brazo.xacro":
  
 
<syntaxhighlight lang="xml">
 
<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" />
  
 
<gazebo_propiedades_joint nombre="base" />
 
<gazebo_propiedades_joint nombre="base" />
Line 839: Line 856:
 
   
 
   
 
<gazebo_propiedades_joint nombre="pinza" />
 
<gazebo_propiedades_joint nombre="pinza" />
−
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
====Creación del fichero de configuración====
+
===Creation of the configuration file===
  
−
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:
+
We have used two [http://wiki.ros.org/robot_mechanism_controllers?distro=electric controllers] for each mobile joint, one for velocity control and one for position control. Each controller is a [http://es.wikipedia.org/wiki/Proporcional_integral_derivativo PID] regulator. We will create a file named "controllers.yaml" within our ''package'' with the content that is shown below in order to configure the [http://es.wikipedia.org/wiki/Proporcional_integral_derivativo PID] regulators of each mobile joint:
  
 
<syntaxhighlight>
 
<syntaxhighlight>
Line 935: Line 951:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Carga del modelo en gazebo===
+
==Loading the model in 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:
+
We will create a file named "brazo_gazebo.launch" within the folder "launch" of our ''package'' with the content that is shown below in order to start [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] and to load the model and the controllers:
  
 
<syntaxhighlight lang="xml" enclose="div">
 
<syntaxhighlight lang="xml" enclose="div">
Line 943: Line 959:
 
<launch>
 
<launch>
  
−
   <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
   <param name="/use_sim_time" value="true" />
  
−
   <param name="robot_description" command="$(find xacro)/xacro.py '$(find brazo_fer_modelo)/brazo.urdf.xacro'" />
+
   <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" />
  
−
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
<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" />
  
−
  <node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot_arm" respawn="false" output="screen" />
+
   <rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>
−
 
−
   <rosparam file="$(find brazo_fer_modelo)/controllers.yaml" command="load"/>
 
  
−
   <node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
+
   <node name="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>
 
</launch>
  
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
Para lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:
+
We will execute the next command in a terminal in order to start the ''launcher'':
  
 
<syntaxhighlight>roslaunch brazo_fer_modelo brazo.gazebo.launch</syntaxhighlight>
 
<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:
+
We can see the arm model in vertical position placed in the point xyz=(0 0 0) of a empty world in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo].
  
−
[[file:modelo_brazo_MYRAbot_gazebo.png|thumb|500px|center|Modelo brazo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+
[[file:modelo_brazo_MYRAbot_gazebo.png|thumb|500px|center|MYRAbot's arm model in [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:
+
We will execute the next command in a new terminal in order to check that the controllers have been loaded properly:
  
 
<syntaxhighlight>rostopic echo</syntaxhighlight>
 
<syntaxhighlight>rostopic echo</syntaxhighlight>
  
−
Debería aparecernos algo similar a lo que se muestra a continuación, donde el ''topic'' NOMBRE_CONTROLADOR/command es al que está suscrito el controlador para recibir el valor de consigna y el ''topic'' NOMBRE_CONTROLADOR/status es en el que publica el estado del controlador:
+
We must obtain something similar to the following, where the ''topic'' NAME_CONTROLLER/command is the set point and the ''topic'' NAME_CONTROLLER/state is the status data:  
  
 
<syntaxhighlight>
 
<syntaxhighlight>
−
/arti1_pos_controller/command
+
/brazo/arti1_pos_controller/command
−
/arti1_pos_controller/state
+
/brazo/arti1_pos_controller/state
−
/arti1_vel_controller/command
+
/brazo/arti1_vel_controller/command
−
/arti1_vel_controller/state
+
/brazo/arti1_vel_controller/state
−
/arti2_pos_controller/command
+
/brazo/arti2_pos_controller/command
−
/arti2_pos_controller/state
+
/brazo/arti2_pos_controller/state
−
/arti2_vel_controller/command
+
/brazo/arti2_vel_controller/command
−
/arti2_vel_controller/state
+
/brazo/arti2_vel_controller/state
−
/arti3_pos_controller/command
+
/brazo/arti3_pos_controller/command
−
/arti3_pos_controller/state
+
/brazo/arti3_pos_controller/state
−
/arti3_vel_controller/command
+
/brazo/arti3_vel_controller/command
−
/arti3_vel_controller/state
+
/brazo/arti3_vel_controller/state
−
/base_pos_controller/command
+
/brazo/base_pos_controller/command
−
/base_pos_controller/state
+
/brazo/base_pos_controller/state
−
/base_vel_controller/command
+
/brazo/base_vel_controller/command
−
/base_vel_controller/state
+
/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
 
/clock
 
/gazebo/link_states
 
/gazebo/link_states
Line 995: Line 1,022:
 
/gazebo/parameter_descriptions
 
/gazebo/parameter_descriptions
 
/gazebo/parameter_updates
 
/gazebo/parameter_updates
−
/gazebo/paused
 
 
/gazebo/set_link_state
 
/gazebo/set_link_state
−
/gazebo/set_model_stat
+
/gazebo/set_model_state
−
/joint_states
 
−
/mechanism_statistics
 
−
/pinza_pos_controller/command
 
−
/pinza_pos_controller/state
 
−
/pinza_vel_controller/command
 
−
/pinza_vel_controller/state
 
 
/rosout
 
/rosout
 
/rosout_agg
 
/rosout_agg
Line 1,009: Line 1,029:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Programa interfaz (gazebo-programas de control)===
+
==Interface program (gazebo-control programs)==
  
−
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:
+
We have created a program that communicates the [[MYRAbot's arm control (bioloid+arduino)#Control programs|arm's control programs]] with [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] controllers. We will create a file named "control_modelo.cpp" within the folder "src" of our ''package'' with the content that is shown below:
  
 
<syntaxhighlight lang="cpp" enclose="div">
 
<syntaxhighlight lang="cpp" enclose="div">
Line 1,030: Line 1,050:
 
ros::NodeHandle n;
 
ros::NodeHandle n;
 
   
 
   
−
   ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("base_pos_controller/command", 1);   
+
   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>("arti1_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>("arti2_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>("arti3_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>("base_vel_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>("arti1_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>("arti2_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>("arti3_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 position = move.posicion;
Line 1,084: Line 1,104:
 
ros::NodeHandle n;
 
ros::NodeHandle n;
 
   
 
   
−
   ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("pinza_pos_controller/command", 1);
+
   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>("pinza_vel_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 position = pinza.posicion;
Line 1,137: Line 1,157:
 
   ros::Subscriber move_sub_= n.subscribe("move_arm", 1, mover);
 
   ros::Subscriber move_sub_= n.subscribe("move_arm", 1, mover);
 
   ros::Subscriber hand_sub_= n.subscribe("hand_arm", 1, pinza);
 
   ros::Subscriber hand_sub_= n.subscribe("hand_arm", 1, pinza);
−
   ros::Subscriber pe_sub_= n.subscribe("joint_states", 1, pe);
+
   ros::Subscriber pe_sub_= n.subscribe("brazo/joint_states", 1, pe);
 
        
 
        
 
   
 
   
−
   ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("base_pos_controller/command", 1);   
+
   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>("arti1_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>("arti2_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>("arti3_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>("pinza_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>("base_vel_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>("arti1_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>("arti2_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>("arti3_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>("pinza_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::Publisher pose_pub_=n.advertise<brazo_fer::ReadServos>("pose_arm", 1);       
Line 1,161: Line 1,181:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 +
We have to add the next line to the file "CMakeLists.txt" of our ''package'' in order to compile and create the executable file:
  
−
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 lang=cmake>rosbuild_add_executable(control_modelo src/control_modelo.cpp)</syntaxhighlight>
  
−
<syntaxhighlight>rosbuild_add_executable(control_modelo src/control_modelo.cpp)</syntaxhighlight>
+
We will execute the next commands in a terminal in order to compile and create the executable file:  
−
 
 
−
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
 
<syntaxhighlight>roscd brazo_fer_modelo
 
make</syntaxhighlight>
 
make</syntaxhighlight>
  
−
==Simulación de programas de control==
+
=Simulation of control programs=
  
−
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>":
+
First we have to add the next code line before the tag "</launch>" within the file "brazo_gazebo.launch" in order to launch the previous interface program:
  
 
<syntaxhighlight lang="xml" enclose="div">
 
<syntaxhighlight lang="xml" enclose="div">
Line 1,181: Line 1,200:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
===Primer programa de control (Ven aquí)===
+
==First control program (Come here)==
  
−
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:
+
We use the same [[MYRAbot's arm control (bioloid+arduino)#Third program (Came here)|control program]] that we had created for the real arm. First we will execute the next command in a terminal in order to launch the simulator with the arm model:
  
 
<syntaxhighlight>roslaunch brazo_fer_modelo brazo_gazebo.launch</syntaxhighlight>
 
<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:
+
When the [http://wiki.ros.org/simulator_gazebo?distro=electric simulator gazebo] is started, we will execute the next command in a new terminal in order to start the control program:
  
 
<syntaxhighlight>rosrun brazo_fer control_v01</syntaxhighlight>
 
<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:
+
We will execute the next command in a new terminal in order to publish the goal point in the ''topic'' "point":
  
 
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 100, y: 0, z: 180}' --once</syntaxhighlight>
 
<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:  
+
The arm will move to the goal point. We can see the arm moving from the start point to the goal point in the next video:
  
 
<center><videoflash>S9kUKYysm0M</videoflash></center>
 
<center><videoflash>S9kUKYysm0M</videoflash></center>
  
−
<center>[http://www.youtube.com/watch?v=S9kUKYysm0M Ver vídeo en YouTube]</center>
+
<center>[http://www.youtube.com/watch?v=S9kUKYysm0M Watch in YouTube]</center>
  
−
===Segundo programa de control (Agarra la lata)===
+
==Second control program (Grasp the tin)==
  
−
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:
+
First we will create a file named "lata.urdf" within the folder "urdf" of our ''package'' with the content that is shown below in order to create the tin model:
  
 
<syntaxhighlight lang="xml">
 
<syntaxhighlight lang="xml">
Line 1,254: Line 1,273:
 
</syntaxhighlight>
 
</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):
+
Also we have to add the next code line to the file "brazo_gazebo.launch" in order to launch the tin model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] in the position 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>
 
<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:
+
We use the same [[MYRAbot's arm control (bioloid+arduino)#Fourth program (Grasp the bottle)|control program]] that we had created for the real arm. First we will execute the next command in a terminal in order to launch the simulator with the arm model:
  
 
<syntaxhighlight>roslaunch brazo_fer_modelo brazo_gazebo.launch</syntaxhighlight>
 
<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:
+
When the [http://wiki.ros.org/simulator_gazebo?distro=electric simulator gazebo] is started, we will execute the next command in a new terminal in order to start the control program:
  
 
<syntaxhighlight>rosrun brazo_fer control_v02</syntaxhighlight>
 
<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:
+
We will execute the next command in a new terminal in order to publish the placement point of the tin in the ''topic'' "pick_point":
  
 
<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 145, y: 0, z: 180}' --once</syntaxhighlight>
 
<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.  
+
The arm will move to the initial position. When we publish the placement point of the tin, the arm will move to grasp the tin. We can see the arm moving to initial position and grasping the tin in the next video:
 +
 
 +
<center><videoflash>16aR8XXHnmw</videoflash></center>
 +
 
 +
<center>[http://www.youtube.com/watch?v=16aR8XXHnmw Watch in YouTube]</center>
 +
 
  
−
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:  
+
----
 +
{| style="border: solid 0px white; width: 100%"
 +
!Related articles
 +
----
 +
!Other articles
 +
----
 +
|-
 +
| valign="top" width="50%" |
  
−
<center><videoflash>16aR8XXHnmw</videoflash></center>
+
[[MYRAbot's arm control (bioloid+arduino)]]<br/>
 +
[[Objects recognition and position calculation (webcam)]]<br/>
 +
[[MYRAbot model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[Voice control (sphinx+festival)]]<br/>
 +
[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 +
 
 +
| valign="top" |
 +
 
 +
[[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/>
 +
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/>
 +
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
 +
[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
 +
[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 +
 
 +
|}
 +
----
  
−
<center>[http://www.youtube.com/watch?v=16aR8XXHnmw Ver vídeo en YouTube]</center>
 
−
---->
 
  
 
[[Mobile manipulation | < go back to main]]
 
[[Mobile manipulation | < go back to main]]

Latest revision as of 11:08, 29 September 2014

< go back to main


Related articles
Other articles

MYRAbot's arm control (bioloid+arduino)
Objects recognition and position calculation (webcam)
MYRAbot model for simulation (urdf+gazebo)
Integration of MYRAbot in moveIt! (gazebo+moveIt!)
Voice control (sphinx+festival)
MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

CeRVaNTeS' arm and base control (maxon+epos2)
CeRVaNTeS' arm model for simulation (urdf+gazebo)
CeRVaNTeS model for simulation (urdf+gazebo)
Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)
CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)


Creation of URDF model

URDF (Unified Robot Description Format) is the format used to define the model of the arm (xml) in order to simulate it in gazebo. It consists of a tree of geometrical elements (links) plugged through joints(joints). This joints can be fixed or mobile, the mobile joints can be rotating, linear or floating.

First, we will create a new package with the necessary dependences for our programs (urdf brazo_fer std_msgs sensor_msgs tf roscpp). We will execute the next commands in a terminal: 

cd ~/ros_workspace
roscreate-pkg brazo_fer_modelo urdf brazo_fer std_msgs sensor_msgs tf roscpp

We have divided the model in three files, main file, macros file and placement file. For the main file, we will create a file named "brazo.xacro" within the folder "urdf" of the created package with the content that is shown below: 

<?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>
  
<!--Muñeca-->
  
  <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 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 móvil-->
  
  <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>

For the macros file, we will create a file named "brazo-macros.xacro" within the folder "src" of our package with the content that is shown below: 

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

For the placement file, we will create a file named "brazo.urdf.xacro" within the folder "src" of our package with the content that is shown below: 

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

Model analysis

As we have used xacro in order to simplify and to make the code more editable, we will execute the next commands in a terminal in order to obtain the URDF file processed: 

roscd brazo_fer_modelo
cd urdf
rosrun xacro xacro.py brazo.urdf.xacro > brazo.urdf

We will execute the next command in a terminal to check the model: 

rosrun urdf_parser check_urdf brazo.urdf

We must obtain something similar to the following: 

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

We will execute the next command in a terminal in order to see the graphic tree generated in a .pdf file: 

rosrun urdf_parser urdf_to_graphiz brazo.urdf

We must obtain something similar to the following:

Árbol de relaciones modelo URBF para el brazo del MYRAbot

Visual test of the model

We will create a file named "brazo_rviz.launch" within the folder "launch" of our package with the content that is shown below in order to display the model and to check the joints: 

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

We will execute the next command in a terminal in order to start the launcher: 

roslaunch brazo_fer_modelo brazo_rviz.launch

The launcher starts rviz and the GUI of joint_state_publisher. We have to set in "Global Options" the field "Fixed Frame" to "/world" link. We can change the position of the joints using the sliders of the GUI of joint_state_publisher. We must obtain something similar to the following:

Loaded model in rviz and the GUI of joint_state_publisher

Control of the joints for simulation

We have to add a controller to each mobile joint in order to simulate the servo motors Dynamixel AX-12A in gazebo

Previous steps

We will execute the next commands in a terminal in order to install the control stacks of the PR2 robot (pr2_simulator, pr2_mechanism and [http://wiki.ros.org/pr2_controllers pr2_controllers): 

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

To add controllers to mobile joints

Modification of the URDF

We have to set the materials, properties, transmissions and controllers for our URDF model in gazebo.

We will add the next code lines before the tag "</robot>" within the file "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>

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

We will add the next code lines before the tag "</macro>" within the file "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" /> 

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

Creation of the configuration file

We have used two controllers for each mobile joint, one for velocity control and one for position control. Each controller is a PID regulator. We will create a file named "controllers.yaml" within our package with the content that is shown below in order to configure the PID regulators of each mobile joint: 

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

Loading the model in gazebo

We will create a file named "brazo_gazebo.launch" within the folder "launch" of our package with the content that is shown below in order to start gazebo and to load the model and the controllers: 

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

We will execute the next command in a terminal in order to start the launcher: 

roslaunch brazo_fer_modelo brazo.gazebo.launch

We can see the arm model in vertical position placed in the point xyz=(0 0 0) of a empty world in gazebo.

MYRAbot's arm model in gazebo

We will execute the next command in a new terminal in order to check that the controllers have been loaded properly: 

rostopic echo

We must obtain something similar to the following, where the topic NAME_CONTROLLER/command is the set point and the topic NAME_CONTROLLER/state is the status data: 

/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

Interface program (gazebo-control programs)

We have created a program that communicates the arm's control programs with gazebo controllers. We will create a file named "control_modelo.cpp" within the folder "src" of our package with the content that is shown below: 

  #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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file: 

rosbuild_add_executable(control_modelo src/control_modelo.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file: 

roscd brazo_fer_modelo
make

Simulation of control programs

First we have to add the next code line before the tag "</launch>" within the file "brazo_gazebo.launch" in order to launch the previous interface program: 

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

First control program (Come here)

We use the same control program that we had created for the real arm. First we will execute the next command in a terminal in order to launch the simulator with the arm model: 

roslaunch brazo_fer_modelo brazo_gazebo.launch

When the simulator gazebo is started, we will execute the next command in a new terminal in order to start the control program: 

rosrun brazo_fer control_v01

We will execute the next command in a new terminal in order to publish the goal point in the topic "point": 

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

The arm will move to the goal point. We can see the arm moving from the start point to the goal point in the next video:

<videoflash>S9kUKYysm0M</videoflash>
Watch in YouTube

Second control program (Grasp the tin)

First we will create a file named "lata.urdf" within the folder "urdf" of our package with the content that is shown below in order to create the tin model: 

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

Also we have to add the next code line to the file "brazo_gazebo.launch" in order to launch the tin model in gazebo in the position 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" />

We use the same control program that we had created for the real arm. First we will execute the next command in a terminal in order to launch the simulator with the arm model: 

roslaunch brazo_fer_modelo brazo_gazebo.launch

When the simulator gazebo is started, we will execute the next command in a new terminal in order to start the control program: 

rosrun brazo_fer control_v02

We will execute the next command in a new terminal in order to publish the placement point of the tin in the topic "pick_point": 

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

The arm will move to the initial position. When we publish the placement point of the tin, the arm will move to grasp the tin. We can see the arm moving to initial position and grasping the tin in the next video:

<videoflash>16aR8XXHnmw</videoflash>
Watch in YouTube


Related articles
Other articles

MYRAbot's arm control (bioloid+arduino)
Objects recognition and position calculation (webcam)
MYRAbot model for simulation (urdf+gazebo)
Integration of MYRAbot in moveIt! (gazebo+moveIt!)
Voice control (sphinx+festival)
MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

CeRVaNTeS' arm and base control (maxon+epos2)
CeRVaNTeS' arm model for simulation (urdf+gazebo)
CeRVaNTeS model for simulation (urdf+gazebo)
Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)
CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)


< go back to main

Retrieved from "https://robotica.unileon.es/index.php?title=MYRAbot%27s_arm_model_for_simulation_(urdf%2bgazebo)&oldid=4216"