View source for Integración de MYRAbot en moveIt! (gazebo+moveIt!)

[[Mobile manipulation | < volver a principal]]


----
{| 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 brazo MYRAbot (urdf+gazebo)]]<br/>
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<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 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 ]]

|}
----


=Pasos previos==

==Instalación de moveIt!==

[http://moveit.ros.org/ MoveIt!] es un ''package'' que proporciona una herramienta software para todo tipo de tareas de manipulación tanto a nivel industrial como doméstico. Está disponible para las distribuciones [http://wiki.ros.org/groovy groovy] e [http://wiki.ros.org/hydro hydro] de [http://wiki.ros.org ROS], por lo que tendremos que tener instalada alguna de ellas. Para instalar [http://moveit.ros.org/ moveIt!] ejecutaremos el siguiente comando en un terminal, donde "ROS_DISTRIBUCION" corresponde con la distribución que tenemos instalada:

<syntaxhighlight>sudo apt-get install ros-ROS_DISTRIBUCION-moveit-full</syntaxhighlight>

==Modificación del modelo de MYRAbot para moveIt!==

===Nuevo URDF del brazo===

Necesitamos modificar el tipo de controladores, de las articulaciones del [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|brazo]], empleados en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] para permitir que pueda tomar el control de estos [http://moveit.ros.org/ moveIt!]. También se han añadido al [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|modelo del brazo]] las mallas 3D de los componentes reales. Crearemos un archivo llamado "brazo-macros_moveit.xacro" dentro del directorio "urdf" del ''package'' "brazo_fer_modelo" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

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

  <property name="M_PI" value="3.14159"/>
  <property name="M_SCALE" value="0.001"/>
  <property name="F10_HEIGHT" value="0.004"/>
  <property name="F4_HEIGHT" value="0.0525"/>
  <property name="F3_HEIGHT" value="0.009"/>
  <property name="AX12_HEIGHT" value="0.0385"/>
  <property name="AX12_WIDTH" value="0.038"/>
  <property name="F2_HEIGHT" value="0.0265"/>
  
  <material name="white">
    <color rgba="0.87 0.90 0.87 1.0"/>
  </material>
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  
<xacro:macro name="default_inertia">
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</xacro:macro>

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

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

  <xacro:macro name="bioloid_F10_fixed" params="parent name color *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
		<default_inertia/>
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package:/brazo_fer_modelo/meshes/F10.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F10.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="finger_fixed" params="parent name color *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
		<default_inertia/>
      </inertial>

      <visual>
        <origin xyz=" 0.03075 0.0 0.001 " rpy="0 0 0" />
        <geometry>
          <box size="0.0865 0.038 0.002" />
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz=" 0.03075 0.0 0.001 " rpy="0 0 0" />
        <geometry>
          <box size="0.0865 0.038 0.002" />
        </geometry>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="bioloid_F3_fixed" params="parent name color *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
		<default_inertia/>
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F3.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F3.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="dynamixel_AX12_fixed" params="parent name *origin">
    <joint name="${name}_joint" type="fixed">
      <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.055" />
        <origin xyz="0 0 0" />
		<default_inertia_servos/>
      </inertial>

      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package:/brazo_fer_modelo/meshes/ax12_box.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="black"/>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <mesh filename="package:/brazo_fer_modelo/meshes/ax12_box.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="bioloid_F3_revolute" params="parent name color llimit ulimit vlimit *origin">
    <joint name="${name}" type="revolute">
      <insert_block name="origin" />
      <axis xyz="0 0 -1"/>
      <limit effort="100" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}" />
	  <default_dynamics/>      
      <parent link="${parent}"/>
      <child link="${name}_brazo_link" />
    </joint>

    <link name="${name}_brazo_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
		<default_inertia/>
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F3.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F3.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="bioloid_F2_revolute" params="parent name color llimit ulimit vlimit *origin">
    <joint name="${name}" type="revolute">
      <insert_block name="origin" />
      <axis xyz="0 1 0"/>
      <limit effort="100" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}" />
	  <default_dynamics/>       
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
		<default_inertia/>
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F2.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F2.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
  </xacro:macro>

  <xacro:macro name="bioloid_F4_revolute" params="parent name color llimit ulimit vlimit *origin">
    <joint name="${name}" type="revolute">
      <insert_block name="origin" />
      <axis xyz="0 1 0"/>
      <limit effort="100" velocity="${vlimit}" lower="${llimit}" upper="${ulimit}" />
	  <default_dynamics/>       
      <parent link="${parent}"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
		<default_inertia/>
      </inertial>

      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F4.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="${color}"/>
      </visual>

      <collision>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F4.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
  </xacro:macro>

<xacro:macro name="gazebo_propiedades_link" params="nombre material">
  <gazebo reference="${nombre}">
	<mu1>100</mu1>
	<mu2>100</mu2>
	<kp>100000000000</kp>
	<kd>1000000000</kd>
	<material>${material}</material>
    <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff> 
  </gazebo>
</xacro:macro>

<xacro:macro name="gazebo_propiedades_joint" params="nombre">
 <gazebo reference="${nombre}">
    <erp>0.1</erp>
    <stopKd value="100000000.0" />
    <stopKp value="100000000.0" />
    <fudgeFactor value="0.5" />
 </gazebo>
</xacro:macro> 

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

  </plugin>
</gazebo>

  <transmission name="base_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base"/>
    <actuator name="base_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <transmission name="arti1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arti1"/>
    <actuator name="arti1_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <transmission name="arti2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arti2"/>
    <actuator name="arti3_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <transmission name="arti3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="arti3"/>
    <actuator name="arti3_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  
  <transmission name="pinza_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="pinza"/>
    <actuator name="pinza_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>  

</robot>

</syntaxhighlight>

Los archivos empleados, con los modelos de malla 3D ".stl", en la descripción del brazo se pueden descargar a continuación, deben guardarse en el directorio "meshes" del ''package'' "brazo_fer_modelo":

* [http://www.fernando.casadogarcia.es/files/ax12_box.stl ax12_box.stl]
* [http://www.fernando.casadogarcia.es/files/F2.stl F2.stl]
* [http://www.fernando.casadogarcia.es/files/F3.stl F3.stl]
* [http://www.fernando.casadogarcia.es/files/F4.stl F4.stl]
* [http://www.fernando.casadogarcia.es/files/F10.stl F10.stl]

Crearemos un archivo llamado "brazo_moveit.xacro" dentro del directorio "urdf" del ''package'' "brazo_fer_modelo" con el siguiente contenido:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">

	<xacro:include filename="$(find brazo_fer_modelo)/urdf/brazo-macros_moveit.xacro" />
    
<xacro:macro name="brazo" params="parent *origin">
	
  <joint name="fixed" type="fixed">
    <parent link="${parent}"/>
    <child link="fixed_link"/>
    <insert_block name="origin" />
    <axis xyz="0 0 1" />  
  </joint>
  
  <link name="fixed_link"/>	

    <!-- shoulder pan joint -->
    <dynamixel_AX12_fixed parent="fixed_link" name="servo_base">
      <origin xyz="0 0 0.019" rpy="${M_PI/2} 0 ${-M_PI/2}"/>
    </dynamixel_AX12_fixed>
    <bioloid_F3_revolute parent="servo_base_link" name="base" color="white" vlimit="3" llimit="-2.62" ulimit="2.62">
      <origin xyz="0 ${AX12_WIDTH/2} 0" rpy="${-M_PI/2} ${-M_PI/2} ${-M_PI}" />
    </bioloid_F3_revolute>

    <!-- shoulder lift joint -->
    <dynamixel_AX12_fixed parent="base_brazo_link" name="servo_arti1">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0" />
    </dynamixel_AX12_fixed>
    <bioloid_F4_revolute parent="servo_arti1_link" name="arti1" color="white" vlimit="3" llimit="-2.62" ulimit="2.62">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </bioloid_F4_revolute>
    <bioloid_F10_fixed parent="arti1_link" name="arti1_F10_0" color="white">
      <origin xyz="0 0 ${F4_HEIGHT+F10_HEIGHT/2}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arti1_F10_0_link" name="arti1_F10_1" color="white">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arti1_F10_1_link" name="arti1_F10_2" color="white">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F3_fixed parent="arti1_F10_2_link" name="arti1_F3_0" color="white">
      <origin xyz="0 0 ${F10_HEIGHT/2}" rpy="0 ${M_PI} 0" />
    </bioloid_F3_fixed>

    <!-- elbow joint -->
    <dynamixel_AX12_fixed parent="arti1_F3_0_link" name="servo_arti2">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
    </dynamixel_AX12_fixed>
    <bioloid_F4_revolute parent="servo_arti2_link" name="arti2" color="white" vlimit="3" llimit="-2.63" ulimit="2.63">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </bioloid_F4_revolute>
    <bioloid_F10_fixed parent="arti2_link" name="arti2_F10_0" color="white">
      <origin xyz="0 0 ${F4_HEIGHT+F10_HEIGHT/2}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arti2_F10_0_link" name="arti2_F10_1" color="white">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F10_fixed parent="arti2_F10_1_link" name="arti2_F10_2" color="white">
      <origin xyz="0 0 ${F10_HEIGHT}" rpy="0 0 0" />
    </bioloid_F10_fixed>
    <bioloid_F3_fixed parent="arti2_F10_2_link" name="arti2_F3_0" color="white">
      <origin xyz="0 0 ${F10_HEIGHT/2}" rpy="0 ${M_PI} 0" />
    </bioloid_F3_fixed>

    <!-- wrist joint -->
    <dynamixel_AX12_fixed parent="arti2_F3_0_link" name="servo_arti3">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0"/>
    </dynamixel_AX12_fixed>
    <bioloid_F2_revolute parent="servo_arti3_link" name="arti3" color="white" vlimit="3" llimit="-2.62" ulimit="2.62">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </bioloid_F2_revolute>
    <bioloid_F3_fixed parent="arti3_link" name="arti3_F3_0" color="white">
      <origin xyz="0 0.016 ${F2_HEIGHT}" rpy="0 ${M_PI} ${-M_PI/2}" />
    </bioloid_F3_fixed>

    <!-- gripper joint -->
    <dynamixel_AX12_fixed parent="arti3_F3_0_link" name="servo_pinza">
      <origin xyz="-0.02275 0 ${-AX12_WIDTH/2}" rpy="${M_PI} ${M_PI/2} 0"/>
    </dynamixel_AX12_fixed>

    <!-- finger 1 -->
    <joint name="pinza" type="revolute">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
      <limit effort="100" velocity="3" lower="-2.62" upper="2.62" />
      <default_dynamics/>
      <parent link="servo_pinza_link"/>
      <child link="pinza_movil_link" />
    </joint>
    <link name="pinza_movil_link">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
        <default_inertia/>      
      </inertial>
      <visual>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F2.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
        <material name="white"/>
      </visual>
      <collision>
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
          <mesh filename="package://brazo_fer_modelo/meshes/F2.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
    </link>
    <finger_fixed parent="pinza_movil_link" name="pinza_movil_dedo" color="white">
      <origin xyz="0 0 ${F2_HEIGHT}" rpy="0 0 0" />
    </finger_fixed>

    <!-- finger 2 -->
    <bioloid_F3_fixed parent="servo_pinza_link" name="pinza_fija" color="white">
      <origin xyz="0 0 ${-AX12_HEIGHT-F10_HEIGHT+0.001}" rpy="0 ${M_PI} 0" />
    </bioloid_F3_fixed>
    <finger_fixed parent="pinza_fija_link" name="pinza_fija_dedo" color="white">
      <origin xyz="0 0 0" rpy="0 0 ${M_PI}" />
    </finger_fixed>


<gazebo_propiedades_link nombre="servo_base_link" material="brazo_fer/Black" />

<gazebo_propiedades_link nombre="base_brazo_link" material="brazo_fer/White" />
  
<gazebo_propiedades_link nombre="servo_arti1_link" material="brazo_fer/Black" />  
  
<gazebo_propiedades_link nombre="arti1_link" material="brazo_fer/White" />
   
<gazebo_propiedades_link nombre="arti1_F10_0_link" material="brazo_fer/White" />
  
<gazebo_propiedades_link nombre="arti1_F10_1_link" material="brazo_fer/White" />   
  
<gazebo_propiedades_link nombre="arti1_F10_2_link" material="brazo_fer/White" />
 
<gazebo_propiedades_link nombre="arti1_F3_0_link" material="brazo_fer/White" />

<gazebo_propiedades_link nombre="servo_arti2_link" material="brazo_fer/Black" />  
  
<gazebo_propiedades_link nombre="arti2_link" material="brazo_fer/White" />
   
<gazebo_propiedades_link nombre="arti2_F10_0_link" material="brazo_fer/White" />
  
<gazebo_propiedades_link nombre="arti2_F10_1_link" material="brazo_fer/White" />   
  
<gazebo_propiedades_link nombre="arti2_F10_2_link" material="brazo_fer/White" /> 

<gazebo_propiedades_link nombre="arti2_F3_0_link" material="brazo_fer/White" /> 

<gazebo_propiedades_link nombre="servo_arti3_link" material="brazo_fer/Black" />  
  
<gazebo_propiedades_link nombre="arti3_link" material="brazo_fer/White" />
   
<gazebo_propiedades_link nombre="arti3_F3_0_link" material="brazo_fer/White" />
 
<gazebo_propiedades_link nombre="servo_pinza_link" material="brazo_fer/Black" /> 

<gazebo_propiedades_link nombre="pinza_movil_link" material="brazo_fer/White" />      
  
<gazebo_propiedades_link nombre="pinza_movil_dedo_link" material="brazo_fer/WhiteTransparent" />

<gazebo_propiedades_link nombre="pinza_fija_link" material="brazo_fer/White" />  

<gazebo_propiedades_link nombre="pinza_fija_dedo_link" material="brazo_fer/WhiteTransparent" />  
  

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

</xacro:macro>
     
</robot>

</syntaxhighlight>

===Controladores para simulación brazo===

Crearemos un archivo llamado "controller_moveit.yaml" dentro del directorio "config" del ''package'' "brazo_fer_modelo" con el siguiente contenido:

<syntaxhighlight>

  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  
  # Position Controllers ---------------------------------------   
  pinza_pos_controller:
    type: effort_controllers/JointPositionController
    joint: pinza
    pid: {p: 0.8, i: 0.6, d: 0.3, i_clamp: 1}

  brazo_controller:
    type: effort_controllers/JointTrajectoryController
    joints:
      - arti1
      - arti2
      - arti3
      - base

    gains: # Required because we're controlling an effort interface
      arti1: {p: 1.0, i: 0.6, d: 0.3, i_clamp: 10}
      arti2: {p: 1.0, i: 0.6, d: 0.3, i_clamp: 10}
      arti3: {p: 1.0, i: 0.6, d: 0.3, i_clamp: 10}
      base: {p: 1.0, i: 0.6, d: 0.3, i_clamp: 10}

</syntaxhighlight>

===Controlador para el brazo real (actionlib-action server)===

Para la interacción entre el [[Control brazo MYRAbot (bioloid+arduino)|brazo]] y [http://moveit.ros.org/ moveIt!] es necesario un [http://wiki.ros.org/actionlib actionlib-ActionServer] que reciba las trayectorias calculadas a través de un ''Goal'', se suscriba al ''topic'' "pose_arm" (estados del brazo) y publique en el ''topic'' "move_arm" (movimiento del brazo). Crearemos un nuevo archivo llamado "controlador_brazo.cpp" dentro del directorio "src" del ''package'' "brazo_fer" con el siguiente contenido:

<syntaxhighlight lang="cpp" enclose="div">

#include <ros/ros.h>
#include <ros/time.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <myrabot_arm_base_b/WriteServos.h>
#include <myrabot_arm_base_b/ReadServos.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

#define PI 3.14159265

class MYRAbotArm
{
public:

  MYRAbotArm(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    as_.registerGoalCallback(boost::bind(&MYRAbotArm::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&MYRAbotArm::preemptCB, this));

    sub_pose_arm_ = nh_.subscribe("/pose_arm", 1, &MYRAbotArm::analysisCB, this);

    pub_move_arm_ = nh_.advertise<myrabot_arm_base_b::WriteServos>("/move_arm", 1, this);

    as_.start();
  }

  ~MYRAbotArm(void)
  {
  }

  void goalCB()
  {
	i_ = 0;

	goal_ = as_.acceptNewGoal()->trajectory;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    as_.setPreempted();
  }

  void analysisCB(const myrabot_arm_base_b::ReadServos& arm)
  {
    if (!as_.isActive())
      return;

    ros::Time ahora = ros::Time::now();

    feedback_.header.stamp = ahora;

    feedback_.joint_names.resize(4);
    feedback_.actual.positions.resize(4);
    feedback_.actual.effort.resize(4);

	for (int i = 0; i < 4; i++)
	{
		switch(i)
		{
			case 3:
				feedback_.joint_names[i] = "base_brazo";
				feedback_.actual.positions[i] = ((((arm.posicion.base * 300)/1023)*PI)/180)-2.62;
				feedback_.actual.effort[i] = ((((arm.corriente.base * 300)/1023)*PI)/180)-2.62;
			break;
			case 0:
				feedback_.joint_names[i] = "arti1";
				feedback_.actual.positions[i] = ((((arm.posicion.arti1 * 300)/1023)*PI)/180)-2.62;
				feedback_.actual.effort[i] = ((((arm.corriente.arti1 * 300)/1023)*PI)/180)-2.62;
			break;
			case 1:
				feedback_.joint_names[i] = "arti2";
				feedback_.actual.positions[i] = ((((arm.posicion.arti2 * 300)/1023)*PI)/180)-2.62;
				feedback_.actual.effort[i] = ((((arm.corriente.arti2 * 300)/1023)*PI)/180)-2.62;
			break;
			case 2:
				feedback_.joint_names[i] = "arti3";
				feedback_.actual.positions[i] = ((((arm.posicion.arti3 * 300)/1023)*PI)/180)-2.62;
				feedback_.actual.effort[i] = ((((arm.corriente.arti3 * 300)/1023)*PI)/180)-2.62;
			break;
		}
	}

	puntos_ = goal_.points.size();

	if (i_ != puntos_)
	{

	    	myrabot_arm_base_b::WriteServos move;

	    		if (i_ == 0
	    		    ||((feedback_.actual.positions[0] <= (goal_.points[i_-1].positions[0] + 0.1) && feedback_.actual.positions[0] >= (goal_.points[i_-1].positions[0] - 0.1))
	    		    && (feedback_.actual.positions[1] <= (goal_.points[i_-1].positions[1] + 0.1) && feedback_.actual.positions[1] >= (goal_.points[i_-1].positions[1] - 0.1))
	    		    && (feedback_.actual.positions[2] <= (goal_.points[i_-1].positions[2] + 0.1) && feedback_.actual.positions[2] >= (goal_.points[i_-1].positions[2] - 0.1))
	    		    && (feedback_.actual.positions[3] <= (goal_.points[i_-1].positions[3] + 0.1) && feedback_.actual.positions[3] >= (goal_.points[i_-1].positions[3] - 0.1))))
	    		{
	    			for (int j = 0; j < 4; j++)
	    			{
	    				switch (j)
	    				{
	    				case 3:
	    					move.posicion.base = ((((goal_.points[i_].positions[j]+2.62)*180)/PI)*1023)/300;
	    					move.velocidad.base = abs(goal_.points[i_].velocities[j]*511);
	    					move.par.base = 1;
	    				break;
	    				case 0:
	    					move.posicion.arti1 = ((((goal_.points[i_].positions[j]+2.62)*180)/PI)*1023)/300;
	    					move.velocidad.arti1 = abs(goal_.points[i_].velocities[j]*511);
	    					move.par.arti1 = 1;
	    				break;
	    				case 1:
	    					move.posicion.arti2 = ((((goal_.points[i_].positions[j]+2.62)*180)/PI)*1023)/300;
	    					move.velocidad.arti2 = abs(goal_.points[i_].velocities[j]*511);
	    					move.par.arti2 = 1;
	    				break;
	    				case 2:
	    					move.posicion.arti3 = ((((goal_.points[i_].positions[j]+2.62)*180)/PI)*1023)/300;
	    				        move.velocidad.arti3 = abs(goal_.points[i_].velocities[j]*511);
	    					move.par.arti3 = 1;
	    				break;
	    				}
	    			}

	    			pub_move_arm_.publish(move);

	    			t_inicio = ros::Time::now();
	    		}
	    		else
	    		{
	    			i_ = i_ - 1;
	    		}

	    		ros::Time ahora = ros::Time::now();

	    		ros::Duration duracion(1.0);

	    		if (ahora > (t_inicio + duracion))
	    		{
	    			result_.error_code = -1;
	    	                as_.setAborted(result_);
	    		}

			feedback_.desired = goal_.points[i_];

			feedback_.error.positions.resize(4);

			for (int j = 0; j < 4; j++)
			{
				feedback_.error.positions[j] = feedback_.desired.positions[j] - feedback_.actual.positions[j];
			}

    		i_ ++;
  	}
	else
	{
		result_.error_code = 0;
	        as_.setSucceeded(result_);
	}
	as_.publishFeedback(feedback_);

  }

protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
  std::string action_name_;
  int puntos_, i_;
  ros::Time t_inicio;
  trajectory_msgs::JointTrajectory goal_;
  control_msgs::FollowJointTrajectoryResult result_;
  control_msgs::FollowJointTrajectoryFeedback feedback_;
  ros::Subscriber sub_pose_arm_;
  ros::Publisher pub_move_arm_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "brazo_controller/follow_join_trajectory");

  MYRAbotArm brazo(ros::this_node::getName());
  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 lang=cmake>rosbuild_add_executable(controlador_brazo src/controlador_brazo.cpp)</syntaxhighlight>

Necesitamos añadir las siguientes dependencias al ''package'' en el archivo "manifest.xml": 

<syntaxhighlight>actionlib
actionlib_msgs
trajectory_msgs
control_msgs</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
make</syntaxhighlight>

===Nuevo URDF soporte y cámara xtion===

Se ha colocado una estructra de soporte para situar la nueva cámara [http://www.asus.com/Multimedia/Xtion_PRO_LIVE/ asus Xtion PRO LIVE] para poder realizar tareas de manipulación y reconocimiento de objetos, en sustitución de la anterior [[Detección y cálculo de posición de objetos (cámara web)|cámara web]]. Para la estructura de soporte añadiremos las siguientes líneas de código al archivo "estructura-myrabot.xacro" situado en el directotio "urdf" del ''package'' "myrabot_fer_modelo":

<syntaxhighlight lang="xml" enclose="div">

...

<joint name="soporte_3" type="fixed">
  <parent link="e_soporte_1_link"/>
  <child link="e_soporte_3_link"/>
  <origin xyz="0 0.065 0.61" rpy="0.12 0 0" />
</joint>
  
<link name="e_soporte_3_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0.0 -0.065 ${0.06/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="-0.135 -0.2 -0.03"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_3.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.135 -0.2 -0.03"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_3.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link> 

...

<gazebo_propiedades nombre="e_soporte_3_link" material="myrabot_fer/LightWood" />

...

</syntaxhighlight>

Crearemos un archivo llamado "xtion.xacro" dentro del directorio "urdf" del ''package'' "myrabot_fer_modelo" con el siguiente contenido para el modelo del sensor 3D:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  
  <!-- Xacro properties -->
  <xacro:property name="M_SCALE" value="0.001"/>
  <xacro:property name="asus_xtion_pro_depth_rel_rgb_py" value="0.0270" />
  <xacro:property name="asus_xtion_pro_cam_rel_rgb_py"   value="-0.0220" />
  
  <property name="M_PI" value="3.14159"/>

  <!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
  <xacro:macro name="xtion" params="parent *origin">
	  
    <joint name="camera_rgb_joint_xtion" type="fixed">
	  <insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="camera_rgb_frame_xtion" />
    </joint>

    <link name="camera_rgb_frame_xtion">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0"
          izz="0.0001" />
      </inertial>
    </link>

    <joint name="camera_rgb_optical_joint_xtion" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="camera_rgb_frame_xtion" />
      <child link="camera_rgb_optical_frame_xtion" />
    </joint>

    <link name="camera_rgb_optical_frame_xtion">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0"
          izz="0.0001" />
      </inertial>
    </link>

    <joint name="camera_joint_xtion" type="fixed">
      <origin xyz="0.0175 ${asus_xtion_pro_cam_rel_rgb_py} 0.021"
              rpy="0 0 0"/>
      <parent link="camera_rgb_frame_xtion"/>
      <child link="camera_link_xtion"/>
    </joint>
    <link name="camera_link_xtion">
      <visual>
        <origin xyz="-0.01 0 0" rpy="${-M_PI/2} -${M_PI} ${-M_PI/2}"/>
        <geometry>
          <mesh filename="package://myrabot_fer_modelo/meshes/asus_xtion_pro_live.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="-0.01 0 0" rpy="${-M_PI/2} -${M_PI} ${-M_PI/2}"/>
        <geometry>
          <mesh filename="package://myrabot_fer_modelo/meshes/asus_xtion_pro_live.dae" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.01" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
          iyy="0.001" iyz="0.0"
          izz="0.001" />
      </inertial>
    </link>

    <joint name="camera_depth_joint_xtion" type="fixed">
      <origin xyz="0.0175 ${asus_xtion_pro_depth_rel_rgb_py} 0.021" rpy="0 0 0" />
      <parent link="camera_rgb_frame_xtion" />
      <child link="camera_depth_frame_xtion" />
    </joint>

    <link name="camera_depth_frame_xtion">
      <inertial>
        <mass value="0.01" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.001" ixy="0.0" ixz="0.0"
          iyy="0.001" iyz="0.0"
          izz="0.001" />
      </inertial>
    </link>

    <joint name="camera_depth_optical_joint_xtion" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="camera_depth_frame_xtion" />
      <child link="camera_depth_optical_frame_xtion" />
    </joint>

    <link name="camera_depth_optical_frame_xtion">
      <inertial>
        <mass value="0.001" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0"
          izz="0.0001" />
      </inertial>
    </link>

    <gazebo reference="camera_link_xtion">  
      <sensor type="depth" name="xtion">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
          <horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
          <image>
            <format>R8G8B8</format>
            <width>640</width>
            <height>480</height>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
        <plugin name="xtion_camera_controller" filename="libgazebo_ros_openni_kinect.so">
          <cameraName>xtion</cameraName>
          <alwaysOn>true</alwaysOn>
          <updateRate>10</updateRate>
          <imageTopicName>rgb/image_raw</imageTopicName>
          <depthImageTopicName>depth/image_raw</depthImageTopicName>
          <pointCloudTopicName>depth/points</pointCloudTopicName>
          <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
          <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
          <frameName>camera_depth_optical_frame_xtion</frameName>
          <baseline>0.1</baseline>
          <distortion_k1>0.0</distortion_k1>
          <distortion_k2>0.0</distortion_k2>
          <distortion_k3>0.0</distortion_k3>
          <distortion_t1>0.0</distortion_t1>
          <distortion_t2>0.0</distortion_t2>
          <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
      </sensor>
    </gazebo>
    
  </xacro:macro>
</robot>

</syntaxhighlight>


Los archivos empleados, con los modelos de malla 3D ".stl" y ".dae", en la descrición de la estructura soporte y de la cámara se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro ''package'':

* [http://www.fernando.casadogarcia.es/files/e_soporte_3.stl e_soporte_3.stl]
* [http://www.fernando.casadogarcia.es/files/asus_xtion_pro_live.dae asus_xtion_pro_live.dae]
* [http://www.fernando.casadogarcia.es/images/asus_xtion_pro_live.png asus_xtion_pro_live.png]

===Nuevo URDF de MYRAbot===

También debemos crear un archivo llamado "myrabot_moveit.urdf.xacro" dentro del directorio "urdf" del ''package'' "myrabot_fer_modelo" con el siguiente contenido, para añadir a el [[Modelo para simulación MYRAbot (urdf+gazebo)|modelo del robot]] el nuevo [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|modelo del brazo]]:

<syntaxhighlight lang="xml" enclose="div">

<?xml version="1.0"?>

<robot name="MYRAbot"
       xmlns:xi="http://www.w3.org/2001/XInclude"
       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
       xmlns:xacro="http://ros.org/wiki/xacro">
	
	<xacro:include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />		
	
	<xacro:include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />
	
	<xacro:include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />	
	
	<xacro:include filename="$(find brazo_fer_modelo)/urdf/brazo_moveit.xacro" />	

	<xacro:include filename="$(find myrabot_fer_modelo)/urdf/xtion.xacro" />
	
	<xacro:include filename="$(find myrabot_fer_modelo)/urdf/ultrasonidos.xacro" />		
	
	<roomba />
	
	<estructura_myrabot parent="base_link">
		<origin rpy="0 0 1.57" xyz="0 0 0.063"/>	
	</estructura_myrabot> 
	
	<kinect parent="e_base_kinect_link">
		<origin rpy="0 0 -1.57" xyz="0 -0.045 0.112"/>
	</kinect>
	
	<brazo parent="e_base_brazo_1_link">
		<origin rpy="0 0 1.57" xyz="0 -0.1015 0.075"/>
	</brazo>
	
	<xtion parent="e_soporte_3_link">
		<origin rpy="0 0.65 -1.57" xyz="0.021 -0.03 0.175"/>	
	</xtion>
	
	<ultrasonidos id="1" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 1.57" xyz="0 0 ${0.04-0.007}"/>	
	</ultrasonidos>	
	
	<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 2.36" xyz="0 0 ${0.04-0.007}"/>	
	</ultrasonidos>	

	<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 3.14" xyz="0 0 ${0.04-0.007}"/>	
	</ultrasonidos>	
	
	<ultrasonidos id="4" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 3.93" xyz="0 0 ${0.04-0.007}"/>
	</ultrasonidos>
	
	<ultrasonidos id="5" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 4.71" xyz="0 0 ${0.04-0.007}"/>
	</ultrasonidos>							

</robot>

</syntaxhighlight>

Crearemos un archivo llamado "myrabot_gazebo_moveit.launcher" dentro del directorio "launch" del ''package'' "myrabot_fer_modelo" con el siguiente contenido, para cargar en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] el nuevo modelo del robot adaptado a [http://moveit.ros.org/ moveIt!]:

<syntaxhighlight lang="xml" enclose="div">

<launch>
  
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="true"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="world_name" value="$(find turtlebot_gazebo)/worlds/empty.world"/>
  </include>  

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

  <node name="spawn_myrabot" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" /> 
 
  
  <rosparam file="$(find brazo_fer_modelo)/config/controller_moveit.yaml" command="load"/>

  <node name="controller_spawner" pkg="controller_manager" type="spawner" args="pinza_pos_controller brazo_controller joint_state_controller" respawn="false" output="screen" />

 
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> 
  

  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="publish_tf" value="true"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="false"/>
    <param name="vo_used" value="false"/>
    <param name="output_frame" value="odom"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
    <param name="max_rate" value="20.0"/>
    <remap from="cloud_in" to="/camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="/camera_depth_frame"/>
    <!-- heights are in the (optical?) frame of the kinect -->
    <param name="min_height" value="-0.15"/>
    <param name="max_height" value="0.15"/>
    <remap from="cloud" to="/cloud_throttled"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="/camera_depth_frame"/>
    <!-- heights are in the (optical?) frame of the kinect -->
    <param name="min_height" value="-0.025"/>
    <param name="max_height" value="0.025"/>
    <remap from="cloud" to="/cloud_throttled"/>
    <remap from="scan" to="/narrow_scan"/>
  </node>   
  
</launch>

</syntaxhighlight>

=Configuración de MYRAbot en moveIt!=

==Asistente de configuración==

Una vez instalado [http://moveit.ros.org/ moveIt!] ejecutaremos el siguiente comando en un terminal para iniciar el asistente de configuración de este ''package'':

<syntaxhighlight>roslaunch moveit_setup_assistant setup_assistant.launch</syntaxhighlight>

Nos aparecerá la ventana que se muestra a continuación, donde podemos escoger entre crear un nuevo ''package'' de configuración (Create New MoveIt Configuration Package) o editar un ''package'' de configuración existente (Edit existing MoveIt Configuration Package):


{| style="border: solid 0px white; width: 100%"
! 
! 
|
|-
|[[file:myrabot_moveit_asistente_01.png|thumb|400px|center|Ventana de inicio asistente [http://moveit.ros.org/ moveIt!]]]
||<center style="text-align: left">Pinchamos sobre la opción "Create New MoveIt Configuration Package" >
<br><br>Seleccionamos el archivo [http://wiki.ros.org/urdf/Tutorials URDF] del [[Modelo para simulación MYRAbot (urdf+gazebo)|modelo de MYRAbot]] pinchando en "Browse" ></center>
|||[[file:myrabot_moveit_asistente_02.png|thumb|400px|center|Ventana creación nuevo ''package'' asistente [http://moveit.ros.org/ moveIt!]]]
|}

Cuando tengamos seleccionado el modelo del robot pincharemos en "Load Files" para que el asistente de [http://moveit.ros.org/ moveIt!] cargue nuestro modelo. Al finalizar el proceso de carga aparecerá el modelo del robot en la parte derecha de la ventana, como se puede ver en la siguiente imagen:

[[file:myrabot_moveit_asistente_03.png|thumb|400px|center|Ventana carga modelo robot asistente [http://moveit.ros.org/ moveIt!]]]

Ahora seleccionaremos en el menú de la parte izquierda de la ventana "Self-Collision" y pincharemos en "Regenerate Default Collision Mtrix" para calcular la matriz de colisiones de nuestro modelo, podemos ver el resultado en la siguiente imagen:

[[file:myrabot_moveit_asistente_04.png|thumb|400px|center|Ventana Self-Collision asistente [http://moveit.ros.org/ moveIt!]]]

En el menú de la izquierda seleccionaremos "Virtual Joints" y pincharemos en "Add Virtual Joint". En el campo "Virtual Joint Name" escribiremos '''virtual_joint''', en el campo "Child_Link" seleccionaremos el ''link'' '''base_footprint''', en el campo "Parent Frame Name" escribiremos '''odom''' y en el campo "Joint Type" seleccionaremos '''planar'''. El procedimiento se muestra en las siguientes imagenes:


{| style="border: solid 0px white; width: 100%"
! 
! 
|
|-
|[[file:myrabot_moveit_asistente_05.png|thumb|400px|center|Ventana añadir "Virtual Joint" asistente [http://moveit.ros.org/ moveIt!]]]
||<center style="text-align: left">Para añadir la "Virtual Joint" pinchamos en "Save" ></center>
|||[[file:myrabot_moveit_asistente_06.png|thumb|400px|center|Ventana "Virtual Joints" asistente [http://moveit.ros.org/ moveIt!]]]
|}

En el menú de la izquierda seleccionaremos "Planning Groups" y pincharemos en "Add group". En el campo "Group Name" escribiremos '''brazo''' y en el campo "Kinematic Solver" seleccionaremos '''kdl_kinematics_plugin/KDLKinematicsPlugin''', el resto de campos se dejarán como viene por defecto. El procedimiento se muestra en las siguientes imagenes:
:

{| style="border: solid 0px white; width: 100%"
! 
! 
|
|-
|[[file:myrabot_moveit_asistente_07.png|thumb|300px|center|Ventana añadir "Group" asistente [http://moveit.ros.org/ moveIt!]]]
||<center style="text-align: left">Para añadir articulaciones pinchamos en "Add Joints" >
<br><br>Seleccionamos de la lista las articulaciones del brazo (<font style="font-weight: bold">base, arti1, arti2, arti3</font>) ></center>
|||[[file:myrabot_moveit_asistente_08.png|thumb|300px|center|Ventana añadir "Joints" a "Group" asistente [http://moveit.ros.org/ moveIt!]]]
||||<center style="text-align: left">Para añadir "Joints" y "Group" pinchamos en "Save" ></center>
|||||[[file:myrabot_moveit_asistente_09.png|thumb|300px|center|Ventana "Planning Group" asistente [http://moveit.ros.org/ moveIt!]]]
|}
También debemos añadir el grupo de la pinza pinchando en "Add Group". En el campo "Group Name" escribiremos '''pinza''' y en el campo "Kinematic Solver" seleccionaremos '''None''', el resto de campos se dejarán como viene por defecto. El procedimiento se muestra en las siguientes imagenes:

{| style="border: solid 0px white; width: 100%"
! 
! 
|
|-
|[[file:myrabot_moveit_asistente_10.png|thumb|300px|center|Ventana añadir "Group" asistente [http://moveit.ros.org/ moveIt!]]]
||<center style="text-align: left">Para añadir partes pinchamos en "Add Links" >
<br><br>Seleccionamos de la lista las partes de la pinza ></center>
|||[[file:myrabot_moveit_asistente_11.png|thumb|300px|center|Ventana añadir "Links" a "Group" asistente [http://moveit.ros.org/ moveIt!]]]
||||<center style="text-align: left">Para añadir "Links" y "Group" pinchamos en "Save" ></center>
|||||[[file:myrabot_moveit_asistente_12.png|thumb|300px|center|Ventana "Planning Group" asistente [http://moveit.ros.org/ moveIt!]]]
|}

Podemos establecer posiciones predefinidas para el brazo y así poder reutilizarlas. En el menú de la izquierda seleccionaremos "Robot Poses" y pincharemos en "Add Pose". En el campo "Pose Name" escribiremos '''home''' y en el campo "Planning Group" seleccionaremos '''brazo'''. Con los controles deslizantes podemos modificar la posición de cada articulación para establecer la posición deseada. El procedimiento se muestra en las siguientes imagenes:

{| style="border: solid 0px white; width: 100%"
! 
! 
|
|-
|[[file:myrabot_moveit_asistente_13.png|thumb|400px|center|Ventana añadir "Robot Pose" asistente [http://moveit.ros.org/ moveIt!]]]
||<center style="text-align: left">Para añadir la "Robot Pose" pinchamos en "Save" ></center>
|||[[file:myrabot_moveit_asistente_14.png|thumb|400px|center|Ventana "Robot Pose" asistente [http://moveit.ros.org/ moveIt!]]]
|}

En el menú de la izquierda seleccionaremos "End Effectors" y pincharemos en "Add End Effector". En el campo "End Effector Name" escribiremos '''pinza_eef''', en el campo "End Effector Group" seleccionaremos '''pinza''', en el campo "Parent Link" seleccionaremos '''arti3_link''' y el campo "Parent Group" lo dejaremos en blanco. El procedimiento se muestra en las siguientes imagenes:

{| style="border: solid 0px white; width: 100%"
! 
! 
|
|-
|[[file:myrabot_moveit_asistente_15.png|thumb|400px|center|Ventana añadir "End Effector" asistente [http://moveit.ros.org/ moveIt!]]]
||<center style="text-align: left">Para añadir el "End Effector" pinchamos en "Save" ></center>
|||[[file:myrabot_moveit_asistente_16.png|thumb|400px|center|Ventana "End Effectors" asistente [http://moveit.ros.org/ moveIt!]]]
|}

Para finalizar seleccionamos "Configuration Files" en el menú de la izquierda y  pincharemos en "Browse" para seleccionar la ubicación y nombre del ''package'' de configuración. Para generar el ''package'' simplemente pincharemos en "Generate Package", como se muestra en la imagen:

[[file:myrabot_moveit_asistente_18.png|thumb|400px|center|Ventana final asistente [http://moveit.ros.org/ moveIt!]]]

==Integración de controladores del brazo==

Crearemos un archivo llamado "controllers.yaml" dentro del directorio "config" del ''package'' de configuración de [http://moveit.ros.org/ moveIt!] creado con el siguiente contenido:

<syntaxhighlight>

controller_list:
 - name: brazo_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
     - arti1
     - arti2
     - arti3
     - base

</syntaxhighlight>

Editaremos el archivo "MYRAbot_moveit_controller_manager.launch.xml" situado en el directorio "launch"  del ''package'' de configuración de [http://moveit.ros.org/ moveIt!] creado para que su contenido sea el siguiente:

<syntaxhighlight lang="xml" enclose="div">

<launch>

 <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
 <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
 <!-- load controller_list -->
 <rosparam file="$(find myrabot_moveit_generated)/config/controllers.yaml"/>

</launch>

</syntaxhighlight>

==Integración de sensores de percepción==

Dentro de estos sensores tenemos la [http://www.asus.com/Multimedia/Xtion_PRO_LIVE/ asus xtion pro live] y la [http://www.microsoft.com/en-us/kinectforwindows/ kinect], con las cuales puede estar equipado MYRAbot y las cuales se configuran de igual modo. Crearemos un archivo llamado "xtion.yaml" dentro del directorio "config" del ''package'' de configuración de [http://moveit.ros.org/ moveIt!] creado con el siguiente contenido:

<syntaxhighlight>

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /xtion/depth/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    filtered_cloud_topic: filtered_cloud

</syntaxhighlight>

Editaremos el archivo "MYRAbot_moveit_sensor_manager.launch.xml" situado en el directorio "launch"  del ''package'' de configuración de [http://moveit.ros.org/ moveIt!] creado para que su contenido sea el siguiente:

<syntaxhighlight lang="xml" enclose="div">

<launch>

 <rosparam command="load" file="$(find myrabot_moveit_generated)/config/xtion.yaml" />
 <param name="octomap_frame" type="string" value="odom" />
 <param name="octomap_resolution" type="double" value="0.05" />
 <param name="max_range" type="double" value="5.0" />

</launch>

</syntaxhighlight>

Tenemos que añadir el valor '''move_group/MoveGroupGetPlanningSceneService''' al parámetro "capabilities" en el archivo "move_group.launch" que se encuantra dentro del directorio "launch"  del ''package'' de configuración de [http://moveit.ros.org/ moveIt!] creado, quedando como se muestra a continuación:

<syntaxhighlight lang="xml" enclose="div">

...

    <!-- MoveGroup capabilities to load -->
    <param name="capabilities" value="move_group/MoveGroupCartesianPathService
				      move_group/MoveGroupExecuteService
				      move_group/MoveGroupKinematicsService
				      move_group/MoveGroupMoveAction
				      move_group/MoveGroupPickPlaceAction
				      move_group/MoveGroupPlanService
				      move_group/MoveGroupQueryPlannersService
				      move_group/MoveGroupStateValidationService
				      move_group/MoveGroupGetPlanningSceneService
				      " />

...

</syntaxhighlight>

==Modificación del archivo .srdf generado==

Tenemos que modificar el archivo "MYRAbot.srdf" generado por el asistente para remplazar las comas por puntos en los valores de las posiciones de las articulaciones guardados, como se muestra a continuación:

<syntaxhighlight lang="xml" enclose="div">

    <group_state name="home" group="brazo">
        <joint name="arti1" value="1,3836" />
        <joint name="arti2" value="-2,2458" />
        <joint name="arti3" value="-0,7065" />
        <joint name="base" value="0" />
    </group_state>

por

    <group_state name="home" group="brazo">
        <joint name="arti1" value="1.3836" />
        <joint name="arti2" value="-2.2458" />
        <joint name="arti3" value="-0.7065" />
        <joint name="base" value="0" />
    </group_state>

</syntaxhighlight>

=Ejecución=

==Planificación simple==

Para probar que MYRAbot funciona correctamente en [http://moveit.ros.org/ moveIt!] podemos iniciar el ''launcher'' "demo.launch" contenido en el directorio "launch" del ''package'' generado, ejecutando en un terminal el siguiente comando:

<syntaxhighlight>roslaunch myrabot_moveit_generated demo.launch</syntaxhighlight>

Se cargará el modelo de MYRAbot en [http://wiki.ros.org/rviz rviz] con el "Motion Planning Interface" de [http://moveit.ros.org/ moveIt!], como se muestra en la siguiente imagen:

[[file:myrabot_moveit_plugin_rviz.png|thumb|500px|center|MYRAbot en [http://wiki.ros.org/rviz rviz] con ''plugin'' de [http://moveit.ros.org/ moveIt!]]]

Podemos ver, si esta seleccionado el modo "interact", unos ''interactive markers'' en torno a la pinza mediante los cuales podremos interactuar con la representación de los estados inicial y final del brazo. Si desplegamos la opción '''Planning Request''' dentro de '''MotionPlanning''', podemos activa o desactivar la representación del estado inicial '''Query Start State''' (representado en verde) o la representación del estado final '''Query Goal State''' (representado en naranja). Cuando alguna de las partes del brazo entre en contacto con otra u otro objeto, estas cambiarán a color rojo.

En el siguiente vídeo podemos ver un proceso de planificación simple, sin obstáculos, que desplaza el brazo desde una posición inicio, en este caso la pose guardada "home", hasta una posición de destino, marcada desplazando el brazo usando los ''interantive markers'':

<center><videoflash>doy6VRjzYZc</videoflash></center>

<center>[http://www.youtube.com/watch?v=doy6VRjzYZc Ver vídeo en YouTube]</center>

==Planificación con escena modelada==

Ejecutaremos el siguiente comando en un terminal para iniciar [http://wiki.ros.org/rviz rviz] con el modelo del robot y el "MotionPlanning" de [http://moveit.ros.org/ moveIt!]:

<syntaxhighlight>roslaunch myrabot_moveit_generated demo.launch</syntaxhighlight>

Se ha creado una escena con figuras geométricas básicas que representan una mesa y una lata sobre esta. No se han empleado mallas 3D, ''meshes'', aunque es una opción disponible, ya que hay problemas de intercambio aleatorio entre las representaciones visuales de los objetos. Crearemos un archivo llamado "mesa_lata.scene" dentro de nuestra carpeta personal
con el siguiente contenido:

<syntaxhighlight>

Mesa_Lata
* mesa_pata_izquierda_delantera
1
cylinder
0.015 0.71
0.20 0.97 0.355
0 0 0 1
0 0 0 1
* mesa_pata_derecha_delantera
1
cylinder
0.015 0.71
0.20 -0.97 0.355
0 0 0 1
0 0 0 1
* mesa_pata_derecha_trasera
1
cylinder
0.015 0.71
0.89 -0.97 0.355
0 0 0 1
0 0 0 1
* mesa_pata_izquierda_trasera
1
cylinder
0.015 0.71
0.89 0.97 0.355
0 0 0 1
0 0 0 1
* mesa_tablero
1
box
0.75 2 0.03
0.545 0 0.725
0 0 0 1
0.71372549 0.494117647 0.301960784 1
* lata
1
cylinder
0.03 0.12
0.28 0 0.8
0 0 0 1
1 0 0 1
.

</syntaxhighlight>

Este archivo lo cargaremos en el menú '''Scene Objects''' de '''MotionPlanning''' pinchando en '''Import From Text'''. En el siguiente vídeo podemos ver el proceso de carga de la escena y la planificación de la trayectoria para evitar la lata, representada por un cilindro de color rojo, situada sobre la masa, desplazando el brazo desde un punto de partida a otro de destino entre los cuales se encuentra situada la lata:

<center><videoflash>oWe_jj4keXU</videoflash></center>

<center>[http://www.youtube.com/watch?v=oWe_jj4keXU Ver vídeo en YouTube]</center>

==Planificación con robot en gazebo==

Crearemos un archivo llamado "myrabot_gazebo_moveit_mesa+latas.launch" dentro del directorio "launch" del ''package'' "myrabot_fer_modelo" con el siguiente contenido, para cargar [[Modelo para simulación MYRAbot (urdf+gazebo)#Creación de modelos de objetos|los modelos de los objetos]] (mesa y latas):

<syntaxhighlight lang="xml" enclose="div">

<launch>
  
  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo_moveit.launch" />

  <!-- Load models table and cans -->
  <node name="spawn_mesa" pkg="gazebo_ros" type="spawn_model" args="-file $(find myrabot_objects_models_b)/urdf/mesa.urdf -urdf -x 0.545 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />    
  
  <!--node name="spawn_lata_coca_cola" pkg="gazebo_ros" type="spawn_model" args="-file $(find myrabot_objects_models_b)/urdf/lata_coca_cola.urdf -urdf -x 0.32 -y -0.18 -z 0.74 -Y -1.57 -model lata_Coca_Cola" respawn="false" output="screen" /-->  

  <node name="spawn_lata_amstel" pkg="gazebo_ros" type="spawn_model" args="-file $(find myrabot_objects_models_b)/urdf/lata_amstel.urdf -urdf -x 0.32 -y 0.0 -z 0.74 -Y -1.57 -model lata_Amstel" respawn="false" output="screen" /> 

</launch>

</syntaxhighlight>  

Crearemos otro archivo llamado "moveit_planning_execution.launch" dentro del directorio "launch" del ''package'' de configuración de [http://moveit.ros.org/ moveIt!] con el siguiente contenido, para iniciar el interfaz de planificación de movimientos:

<syntaxhighlight lang="xml" enclose="div">

<launch>
 # The planning and execution components of MoveIt! configured to 
 # publish the current configuration of the robot (simulated or real)
 # and the current state of the world as seen by the planner
 <include file="$(find myrabot_moveit_generated)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>
 # The visualization component of MoveIt!
 <include file="$(find myrabot_moveit_generated)/launch/moveit_rviz.launch"/>
</launch>

</syntaxhighlight>  

Ejecutaremos el siguiente comando en un terminal para iniciar [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] con los modelos del robot y los objetos:

<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo_moveit_mesa+latas.launch</syntaxhighlight>

En otro terminal ejecutaremos el siguiente comando para iniciar [http://wiki.ros.org/rviz rviz] con el "Motion Planning Interface" de [http://moveit.ros.org/ moveIt!]:

<syntaxhighlight>roslaunch myrabot_moveit_generated moveit_planning_execution.launch</syntaxhighlight>

En el siguiente vídeo podemos ver el proceso de planificación y ejecución de la trayectoria para evitar la lata que se encuentra en medio de la trayectoria de las posiciones de inicio y destino, se planifica dos veces ya que la primera vez se detecta una colisión entre el brazo y la lata. La escena representada corresponde con la captada por el sensor de percepción situado por encima de la pantalla, donde se muestra el espacio ocupado mediante cubos con una tolerancia que permita evitar colisiones.

<center><videoflash>xf1Jz1L-Xyc</videoflash></center>

<center>[http://www.youtube.com/watch?v=xf1Jz1L-Xyc Ver vídeo en YouTube]</center>

==Planificación simple con robot real==

Crearemos un archivo llamado "controlador_moveit.launch" dentro del directorio "launch" del ''package'' "brazo_fer" con el siguiente contenido, para cargar el modelo del robot e iniciar el brazo con el controlador para [http://moveit.ros.org/ moveIt!]:

<syntaxhighlight lang="xml" enclose="div">

<launch>

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot_moveit.urdf.xacro'" />
  
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"></node>  

  <node name="serial_node" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM0" />

  <node name="joint_states_brazo" pkg="brazo_fer" type="joint_states_brazo" /> 
  
  <group ns="brazo_controller">

    <node name="follow_joint_trajectory" pkg="brazo_fer" type="controlador_moveit" output="screen" />
  
  </group>

</launch>

</syntaxhighlight>

Ejecutaremos el siguiente comando en un terminal para iniciar el control del brazo:

<syntaxhighlight>roslaunch brazo_fer controlador_moveit.launch</syntaxhighlight>

En otro terminal ejecutaremos el siguiente comando para iniciar [http://wiki.ros.org/rviz rviz] con el "Motion Planning Interface" de [http://moveit.ros.org/ moveIt!]:

<syntaxhighlight>roslaunch myrabot_moveit_generated moveit_planning_execution.launch</syntaxhighlight>

En el siguiente vídeo puede verse la planificación y ejecución de las trayectorias, por el brazo real, para alcanzar las pociones marcadas por el modelo del robot haciendo uso de ''interactive markers'':

<center><videoflash>Kz7bKL4ZP98</videoflash></center>

<center>[http://www.youtube.com/watch?v=Kz7bKL4ZP98 Ver vídeo en YouTube]</center>


----
{| 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 brazo MYRAbot (urdf+gazebo)]]<br/>
[[Modelo para simulación MYRAbot (urdf+gazebo)]]<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 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]]