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

From robotica.unileon.es
Jump to: navigation, search
(→‎Edition of the plugin for create of the turtlebot)
Line 1,153: Line 1,153:
  
 
===URDF kinect===
 
===URDF kinect===
 +
 
<!--
 
<!--
  

Revision as of 16:28, 12 February 2014

< go back to main

Components of MYRAbot model

We have started creating a model of the MYRAbot's structure and we have added the mobile base Roomba, the kinect camera, the arm model and the webcam Logitech Webcam Pro 9000.

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

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

URDF MYRAbot's structure

We have made the 3D model of the structure with a CAD software from which we use the parts with a complex geometry.

3D CAD design of MYRAbot's structure

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

<?xml version="1.0"?>

<robot>

	<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot-macros.xacro" />

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

<joint name="world_joint" type="fixed">
  <parent link="${parent}"/>
  <child link="e_base_1_link"/>
  <insert_block name="origin" />
</joint>
       
  <link name="e_base_1_link">
    <inertial>
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.015" />
      </geometry>
      <material name="wood">
		<color rgba="${205/255} ${133/255} ${63/255} 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.015" />
      </geometry>
    </collision>
  </link> 

<separador_base id="1" />
<e_separador_base id="1" xsim="1" ysim="1" />

<separador_base id="2" />
<e_separador_base id="2" xsim="1" ysim="-1" />

<separador_base id="3" />
<e_separador_base id="3" xsim="-1" ysim="-1" />

<separador_base id="4" />
<e_separador_base id="4" xsim="-1" ysim="1" />


<joint name="base_2" type="fixed">
  <parent link="e_base_1_link"/>
  <child link="e_base_2_link"/>
  <origin xyz="0 0 0.135" rpy="0 0 0" />
</joint>

  <link name="e_base_2_link">
    <inertial>
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.02" />
      </geometry>
      <material name="wood">
		<color rgba="${205/255} ${133/255} ${63/255} 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.02" />
      </geometry>
    </collision>
  </link>

<joint name="pilar" type="fixed">
  <parent link="e_base_2_link"/>
  <child link="e_pilar_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
  
<link name="e_pilar_link">
    <inertial>
      <mass value="0.08"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.7/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
 
<joint name="ultrasonidos" type="fixed">
  <parent link="e_base_2_link"/>
  <child link="e_ultrasonidos_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
  
<link name="e_ultrasonidos_link">
    <inertial>
      <mass value="0.04"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 ${0.04/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

<joint name="botonera" type="fixed">
  <parent link="e_base_2_link"/>
  <child link="e_botonera_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
  
<link name="e_botonera_link">
    <inertial>
      <mass value="0.06"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
      <geometry>
        <box size="0.2 0.09 0.1"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
      <geometry>
        <box size="0.2 0.09 0.1"/>
      </geometry>
    </collision>
  </link>
  
<joint name="start_button_1" type="fixed">
  <parent link="e_botonera_link"/>
  <child link="e_start_button_1_link"/>
  <origin xyz="-0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_start_button_1_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
      <geometry>
        <cylinder radius="0.009" length="0.002"/>
      </geometry>
      <material name="black">
		  <color rgba="0 0 0 1"/>
      </material>		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
      <geometry>
        <cylinder radius="0.009" length="0.002"/>
      </geometry>
    </collision>
  </link>
  
<joint name="start_button_2" type="fixed">
  <parent link="e_start_button_1_link"/>
  <child link="e_start_button_2_link"/>
  <origin xyz="0.0 0 0.002" rpy="0 0 0" />
</joint>

<link name="e_start_button_2_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.004"/>
      </geometry>
      <material name="green">
		  <color rgba="0 1 0 1"/>
      </material>		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.004"/>
      </geometry>
    </collision>
  </link>
  
<joint name="emergency_button_1" type="fixed">
  <parent link="e_botonera_link"/>
  <child link="e_emergency_button_1_link"/>
  <origin xyz="0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_1_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.0125" length="0.015"/>
      </geometry>
      <material name="metal" />		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.0125" length="0.015"/>
      </geometry>
    </collision>
  </link>
  
<joint name="emergency_button_2" type="fixed">
  <parent link="e_emergency_button_1_link"/>
  <child link="e_emergency_button_2_link"/>
  <origin xyz="0.0 0 0.015" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_2_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
      <geometry>
        <cylinder radius="0.018" length="0.003"/>
      </geometry>
      <material name="red">
		  <color rgba="1 0 0 1"/>
      </material>		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
      <geometry>
        <cylinder radius="0.018" length="0.003"/>
      </geometry>
    </collision>
  </link>              

<joint name="base_kinect" type="fixed">
  <parent link="e_pilar_link"/>
  <child link="e_base_kinect_link"/>
  <origin xyz="0 0 ${0.395-0.155}" rpy="0 0 0" />
</joint>
  
<link name="e_base_kinect_link">
    <inertial>
      <mass value="0.02"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
<joint name="base_brazo_1" type="fixed">
  <parent link="e_pilar_link"/>
  <child link="e_base_brazo_1_link"/>
  <origin xyz="0 0 ${0.708-(0.395-0.155)}" rpy="0 0 0" />
</joint>
  
<link name="e_base_brazo_1_link">
    <inertial>
      <mass value="0.03"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

<separador_base_brazo id="1" />
<e_separador_base_brazo id="1" xsim="1" ysim="1" />

<separador_base_brazo id="2" />
<e_separador_base_brazo id="2" xsim="1" ysim="-1" />

<separador_base_brazo id="3" />
<e_separador_base_brazo id="3" xsim="-1" ysim="-1" />

<separador_base_brazo id="4" />
<e_separador_base_brazo id="4" xsim="-1" ysim="1" />

<joint name="base_brazo_2" type="fixed">
  <parent link="e_base_brazo_1_link"/>
  <child link="e_base_brazo_2_link"/>
  <origin xyz="0 0 0.074" rpy="0 0 0" />
</joint>
  
<link name="e_base_brazo_2_link">
    <inertial>
      <mass value="0.005"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>    

<joint name="soporte_1" type="fixed">
  <parent link="e_pilar_link"/>
  <child link="e_soporte_1_link"/>
  <origin xyz="0 0 ${0.995-(0.708-0.155)-(0.395-0.155)}" rpy="0 0 0" />
</joint>
  
<link name="e_soporte_1_link">
    <inertial>
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.64/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
<joint name="soporte_2" type="fixed">
  <parent link="e_soporte_1_link"/>
  <child link="e_soporte_2_link"/>
  <origin xyz="0 0.2 0.61" rpy="-0.12 0 0" />
</joint>
  
<link name="e_soporte_2_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.06/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>   

<joint name="monitor" type="fixed">
  <parent link="e_soporte_2_link"/>
  <child link="e_monitor_link"/>
  <origin xyz="0 -0.2 -0.09" rpy="0 0 0" />
</joint>
  
<link name="e_monitor_link">
    <inertial>
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.180/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
<joint name="pantalla" type="fixed">
  <parent link="e_monitor_link"/>
  <child link="e_pantalla_link"/>
  <origin xyz="0.135 ${0.135-0.005-0.03-0.0001} 0.086" rpy="0 0 0" />
</joint>
  
<link name="e_pantalla_link">
    <inertial>
      <mass value="0.0001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.22 0.0001 0.130" />
      </geometry>
      <material name="black" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.22 0.0001 0.127" />
      </geometry>
    </collision>
  </link>       
 
<gazebo_propiedades nombre="e_base_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_separador_base_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_4_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_4_link" material="Grey" />

<gazebo_propiedades nombre="e_pilar_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_kinect_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_botonera_link" material="LightWood" />

<gazebo_propiedades nombre="e_start_button_1_link" material="Black" />

<gazebo_propiedades nombre="e_start_button_2_link" material="Green" />

<gazebo_propiedades nombre="e_emergency_button_1_link" material="Grey" />

<gazebo_propiedades nombre="e_emergency_button_2_link" material="Red" />

<gazebo_propiedades nombre="e_soporte_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_soporte_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_monitor_link" material="LightWood" />

<gazebo_propiedades nombre="e_pantalla_link" material="Black" />

</macro>
  
</robot>

We will add the next files within the folder "meshes" of our package:

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

<?xml version="1.0"?>

<robot>

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

<macro name="separador_base" params="id"> 
<joint name="separador_base_${id}" type="fixed">
  <parent link="e_base_1_link"/>
  <child link="e_separador_base_${id}_link"/>
  <origin xyz="0.135 0.135 0.015" rpy="0 0 0" />
</joint>
</macro>

<macro name="e_separador_base" params="id xsim ysim">
  <link name="e_separador_base_${id}_link">
    <inertial>
      <mass value="0.02"/>
      <origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.120" />
      </geometry>
      <material name="metal">
		<color rgba="${211/255} ${211/255} ${211/255} 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.120" />
      </geometry>
    </collision>
  </link> 
</macro> 

<macro name="separador_base_brazo" params="id"> 
<joint name="separador_base_brazo_${id}" type="fixed">
  <parent link="e_base_brazo_1_link"/>
  <child link="e_separador_base_brazo_${id}_link"/>
  <origin xyz="0.135 ${0.135-0.082} 0.075" rpy="0 0 0" />
</joint>
</macro> 

<macro name="e_separador_base_brazo" params="id xsim ysim">
  <link name="e_separador_base_brazo_${id}_link">
    <inertial>
      <mass value="0.002"/>
      <origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
      <geometry>
        <cylinder radius="0.002" length="0.034" />
      </geometry>
      <material name="metal" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
      <geometry>
        <cylinder radius="0.002" length="0.034" />
      </geometry>
    </collision>
  </link> 
</macro>

<macro name="gazebo_propiedades" params="nombre material">
<gazebo reference="${nombre}">
  <material>Gazebo/${material}</material>
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo> 
</macro>

</robot>

URDF Roomba

We have used the model of the mobile base create of the robot turtlebot, because the vacuum Roomba has similar sensors and physics. We have to install the stacks turtlebot and turtlebot_simulator that contain the robot model and the plugins for gazebo. We have divided the model in two files, main file and plugins file. For the main file, we will create a file named "roomba.xacro" within the folder "urdf" of the created package with the content that is shown below: 

<?xml version="1.0"?>

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


  <include filename="$(find myrabot_fer_modelo)/urdf/roomba-gazebo.xacro"/>


<xacro:macro name="roomba">

    <material name="Green">
      <color rgba="0.0 0.8 0.0 1.0"/>
    </material>

  <link name="base_footprint">
    <inertial>
      <mass value="0.0001" />
      <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>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.001 0.001 0.001" />
      </geometry>
      <material name="Green" />
    </visual>

    <collision>
      <origin xyz="0 0 0.017" rpy="0 0 0" />
      <geometry>
       <box size="0.001 0.001 0.001" />
      </geometry>
    </collision>
  </link>

  <link name="base_link">
    <inertial>
      <mass value="3.6" />
      <origin xyz="0 0 ${0.063/2}" />
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 ${0.063/2}" rpy="0 0 0" />
      <geometry>
        <cylinder length="0.063" radius="0.17"/>
      </geometry>
      <material name="white">
	    <color rgba="1 1 1 1" />  
      </material>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 ${0.063/2}" rpy="0 0 0" />
      <geometry>
        <cylinder length="0.063" radius="0.17"/>
      </geometry>
    </collision>
  </link>

  <link name="wall_sensor_link">
    <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>


  <link name="left_cliff_sensor_link">
    <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>

  <link name="right_cliff_sensor_link">
    <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>



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

  <link name="rightfront_cliff_sensor_link">
    <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="base_footprint_joint" type="fixed">
    <origin xyz="0 0 0.017" rpy="0 0 0" />        
    <parent link="base_footprint"/>
    <child link="base_link" />
  </joint>

  <joint name="base_wall_sensor_joint" type="fixed">
    <origin xyz="0.09 -0.120 0.042" rpy="0 0 -1.0" />        
    <parent link="base_link"/>
    <child link="wall_sensor_link" />
  </joint>

  <joint name="base_left_cliff_sensor_joint" type="fixed">
    <origin xyz="0.07 0.14 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="left_cliff_sensor_link" />
  </joint>

  <joint name="base_right_cliff_sensor_joint" type="fixed">
    <origin xyz="0.07 -0.14 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="right_cliff_sensor_link" />
  </joint>

  <joint name="base_leftfront_cliff_sensor_joint" type="fixed">
    <origin xyz="0.15 0.04 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="leftfront_cliff_sensor_link" />
  </joint>

  <joint name="base_rightfront_cliff_sensor_joint" type="fixed">
    <origin xyz="0.15 -0.04 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="rightfront_cliff_sensor_link" />
  </joint>

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

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </collision>
  </link>

  <joint name="left_wheel_joint" type="continuous">
    <origin xyz="0 0.13 0.015" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

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

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </collision>
  </link>

  <joint name="right_wheel_joint" type="continuous">
    <origin xyz="0 -0.13 0.015" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

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

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.015" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.015" />
      </geometry>
    </collision>
  </link>

  <joint name="rear_castor_joint" type="fixed">
    <origin xyz="-0.13 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="rear_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

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

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </collision>
  </link>

  <joint name="front_castor_joint" type="fixed">
    <origin xyz="0.13 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="front_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>


  <joint name="gyro_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0 0 0.04" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="gyro_link"/>
  </joint>
  <link name="gyro_link">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
    </inertial>
  </link>

  <joint name="laser_joint" type="fixed">
    <origin xyz="-0.065 0 0.57" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="laser" />
  </joint>

  <link name="laser" />
  
<gaezebo>
  <material>Gazebo/White</material>
</gaezebo>  
  
  <myrabot_sim_imu/>
  <myrabot_sim_laser/>
  <myrabot_sim_roomba/>
  <myrabot_sim_wall_sensors/>  
  
</xacro:macro>
  
</robot>

For the plugins file, we will create a file named "roomba-gazebo.xacro" within the folder "urdf" of our package with the content that is shown below: 

<?xml version="1.0"?>

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


<xacro:macro name="myrabot_sim_imu">
  <gazebo>
    <controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
      <robotNamespace>/</robotNamespace>
      <alwaysOn>true</alwaysOn>
      <updateRate>30</updateRate>
      <bodyName>gyro_link</bodyName>
      <topicName>imu/data</topicName>
      <gaussianNoise>${0.0017*0.0017}</gaussianNoise>
      <xyzOffsets>0 0 0</xyzOffsets> 
      <rpyOffsets>0 0 0</rpyOffsets>
      <interface:position name="imu_position"/>
    </controller:gazebo_ros_imu>
  </gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_laser">
  <gazebo reference="laser">
    <sensor:ray name="laser">
      <rayCount>180</rayCount>
      <rangeCount>180</rangeCount>
      <laserCount>1</laserCount>
      
      <origin>0.0 0.0 0.0</origin>
      <displayRays>false</displayRays>
      
      <minAngle>-${60/2}</minAngle>
      <maxAngle>${60/2}</maxAngle>
      
      <minRange>0.08</minRange>
      <maxRange>5.0</maxRange>
      <resRange>0.01</resRange>
      <updateRate>20</updateRate>
      <controller:gazebo_ros_laser name="gazebo_ros_laser_controller" plugin="libgazebo_ros_laser.so">
	<robotNamespace>/</robotNamespace>
        <gaussianNoise>0.005</gaussianNoise>
	<alwaysOn>true</alwaysOn>
	<updateRate>20</updateRate>
	<topicName>scan</topicName>
	<frameName>laser</frameName>
	<interface:laser name="gazebo_ros_laser_iface" />
      </controller:gazebo_ros_laser>
    </sensor:ray>
  </gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_roomba">
  <gazebo>
    <controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_roomba.so">
      <alwaysOn>true</alwaysOn>
      <node_namespace>turtlebot_node</node_namespace>
      <left_wheel_joint>left_wheel_joint</left_wheel_joint>
      <right_wheel_joint>right_wheel_joint</right_wheel_joint>
      <front_castor_joint>front_castor_joint</front_castor_joint>
      <rear_castor_joint>rear_castor_joint</rear_castor_joint>
      <wheel_separation>.260</wheel_separation>
      <wheel_diameter>0.066</wheel_diameter>
      <base_geom>base_link_geom_base_link</base_geom>
      <updateRate>40</updateRate>
      <torque>1.0</torque>
    </controller:gazebo_ros_roomba>
  </gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_wall_sensors">
  <gazebo reference="wall_sensor_link">
    <sensor:ray name="wall_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.0160</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>

  <gazebo reference="left_cliff_sensor_link">
    <sensor:ray name="left_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>

  <gazebo reference="right_cliff_sensor_link">
    <sensor:ray name="right_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>


  <gazebo reference="leftfront_cliff_sensor_link">
    <sensor:ray name="leftfront_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>

  <gazebo reference="rightfront_cliff_sensor_link">
    <sensor:ray name="rightfront_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>fan</displayRays>
    </sensor:ray>
  </gazebo>



  <gazebo reference="left_wheel_link">
    <mu1 value="10"/>
    <mu2 value="10"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
    <fdir value="1 0 0"/>
  </gazebo>

  <gazebo reference="right_wheel_link">
    <mu1 value="10"/>
    <mu2 value="10"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
    <fdir value="1 0 0"/>
  </gazebo>
  <gazebo reference="rear_wheel_link">
    <mu1 value="0"/>
    <mu2 value="0"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
  </gazebo>

  <gazebo reference="front_wheel_link">
    <mu1 value="0"/>
    <mu2 value="0"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
  </gazebo>
</xacro:macro>
</robot>

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

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

roslaunch myrabot_fer_modelo myrabot_gazebo.launch

Si todo va bien podremos ver en la ventana gráfica de gazebo el modelo del MYRAbot situado en un mundo vacío, tal como se muestra en la siguiente imagen:

Modelo MYRAbot en gazebo

Programa de prueba (Vuela al ruedo)

Se ha realizado un pequeño programa para probar el funcionamiento del modelo del MYRAbot, que simplemente hace girar en circulo al robot y gira el brazo de un lado a otro en posición vertical, cuando se acciona el pulsado de inicio del robot. Primero vamos a crear un package que va a contener nuestro programa y que llamaremos "myrabot_fer", con las siguientes dependencias: brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp.

Dentro de este package vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido: 

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "geometry_msgs/Twist.h"
  #include "std_msgs/Bool.h"  

  int cont = 0;
  int cont1= 0;
  int encendido = 0;
  brazo_fer::Servos move, on, vel;
  

  void home()
  {
	if (cont1 == 0)
	{
		ros::NodeHandle n;

		ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
		ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  		
		ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);   
		
		brazo_fer::WriteServos home;
		
		::move.base = 511;
		::move.arti1 = 511;
		::move.arti2 = 511;
		::move.arti3 = 511;
		::move.pinza = 511;
		
		::on.base = 1;
		::on.arti1 = 1;
		::on.arti2 = 1;
		::on.arti3 = 1;
		::on.pinza = 1;
		
		::vel.base = 400;
		::vel.arti1 = 400;
		::vel.arti2 = 400;
		::vel.arti3 = 400;
		::vel.pinza = 400;
		
		home.posicion = ::move;
		home.par = ::on;
		home.velocidad = ::vel;
		
		move_arm_pub_.publish(home);
		hand_pub_.publish(home);
		
		::cont1 = 1;
		
	}
  }
  
  void inicio(const std_msgs::Bool& inicio)
  {
	if (inicio.data == true && ::cont == 0)
	{
		::encendido = 1;
		::cont = 1;
	}
	else if (inicio.data == true && ::cont == 1)
	{
		::encendido = 0;
		::cont = 0;
	}
  }
  
  void posicion(const brazo_fer::ReadServos& posicion)
  {
	ros::NodeHandle n;

  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);    	 	
	
	brazo_fer::Servos p = posicion.posicion;
	brazo_fer::WriteServos mover_brazo;
	
	home();
	
	if (::encendido == 1)
	{	
		geometry_msgs::Twist mover_base;
		
		mover_base.linear.x = 0.1;
		mover_base.angular.z = 0.3;
		
		move_base_pub_.publish(mover_base);		
		
		if (::cont1 ==  1)
		{
			::move.base = 718;
			::cont1 = 2;
			mover_brazo.posicion = ::move;
			mover_brazo.par = ::on;
			mover_brazo.velocidad = ::vel;
			move_arm_pub_.publish(mover_brazo);
		}
		else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))		
		{
			if (::cont1 ==  2)
			{
				::move.base = 304;
				::cont1 = 3;
				mover_brazo.posicion = ::move;
				mover_brazo.par = ::on;
				mover_brazo.velocidad = ::vel;
				move_arm_pub_.publish(mover_brazo);				
			}
			else
			{
				::cont1 = 1;
			}
		}
	}
  }

  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "vuelta_al_ruedo");   
 
	ros::NodeHandle n;
 
 
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion); 
  	ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio); 
 
  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);  	 	   
 
	ros::spin();  
 
        return 0;
  }

Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro package: 

rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)

Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal: 

roscd myrabot_fer
make

Para lanzar gazebo con el modelo del robot en un mundo vacío, ejecutaremos el siguiente comando en un terminal: 

roslaunch myrabot_fer_modelo myrabot_gazebo.launch

Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando: 

rosrun myrabot_fer vuelta_al_ruedo

El modelo del robot permanecerá detenido hasta que publiquemos el valor "true" en el topic "start_button", que corresponde al botón de inicio del robot. Para simular la pulsación del botón de inicio vamos a ejecutar el siguiente comando en un nuevo terminal: 

rostopic pub start_button std_msgs/Bool '{data: true}' --once

Si todo a funcionado bien el robot comenzara a desplazarse en círculo y girar el brazo de un lado a otro en posición vertical, como se muestra en el vídeo:

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

Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja

Para la ejecución del programa escoja en el simulador gazebo usando el modelo del MYRAbot, necesitamos crear previamente los modelos de los objetos a reconocer y una mesa, sobre la cual estarán situados.

Creación de modelos de objetos

Mesa

Primero vamos crear el modelo de la mesa donde vamos a situar los objetos. Se trata de una mesa con tablero de madera y patas metálicas negras de dimensiones 200x75x74 cm (largo x ancho x alto).

Modelo mesa en gazebo

Para contener los objetos vamos a crear un package que llamaremos "objetos_fer_modelos" con las siguientes dependencias: geometry_msgs gazebo urdf roscpp. Para el modelo de la mesa crearemos un archivo llamado "mesa.xacro" dentro del directorio "urdf" del package que acabamos de crear, con el siguiente contenido: 

<?xml version="1.0"?>

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

<link name="world" />

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

<link name="mesa_link">
  <visual>
    <geometry>
      <box size="2 0.75 0.03" /> 
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
      <material name="wood">
		<color rgba="${205/255} ${133/255} ${63/255} 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
    <geometry>
      <box size="2 0.75 0.03" /> 
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
    <mass value="5" />
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>  
  </inertial>
</link>


<xacro:macro name="pata" params="id refx refy">
<joint name="pata_${id}" type="fixed">
  <parent link="mesa_link"/>
  <child link="pata_${id}_link"/>
  <origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
</joint>

<link name="pata_${id}_link">
  <visual>
    <geometry>
      <cylinder radius="0.025" length="0.71" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
    <material name="black">
      <color rgba="0 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
    <geometry>
      <cylinder radius="0.025" length="0.71" /> 
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
    <mass value="3" />
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>  
  </inertial>
</link>
</xacro:macro>

<xacro:pata id="1" refx="1" refy="1"/>
<xacro:pata id="2" refx="1" refy="-1"/>
<xacro:pata id="3" refx="-1" refy="-1"/>
<xacro:pata id="4" refx="-1" refy="1"/>

<gazebo reference="mesa_link">
  <material>Gazebo/LightWood</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<xacro:macro name="gazebo_pata" params="id">
<gazebo reference="pata_${id}_link">
  <material>PR2/Black</material>
  <mu1>0.9</mu1>
  <mu2>0.9</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>
</xacro:macro>

<xacro:gazebo_pata id="1"/>
<xacro:gazebo_pata id="2"/>
<xacro:gazebo_pata id="3"/>
<xacro:gazebo_pata id="4"/>

</robot>

Para el modelo de la mesa hemos empleado los materiales que vienen por defecto en gazebo, pero para las latas vamos a necesitar crear materiales nuevos con las texturas de las etiquetas.

Creación de nuevos materiales para gazebo

Comenzaremos creando dentro de nuestro package un directorio llamado "Media", dentro del cual crearemos otro directorio llamado "materials", y dentro de este crearemos otros dos directorios llamados "scripts" y "textures". También debemos añadir las siguientes líneas al archivo "manifest.xml" de nuestro package, para colocarlo en la ruta de gazebo: 

 <export>
     <gazebo gazebo_media_path="${prefix}" />
 </export>


Los materiales se definen para el motor gráfico OGRE. Para las latas vamos a necesitar asociar una imagen al material para que aplique una textura a la geometría. La imagen que usaremos para la textura debe guardarse en la carpeta "textures" creada dentro de nuestro package. Crearemos un archivo llamado "myrabot_fer.material" dentro del directorio "scripts" de nuestro package con el contenido que se muestra a continuación, para la definición de un nuevo material con textura: 

material material_fer/NOMBRE_DEL_NUEVO_MATERIAL
{
	technique
	{
		pass
		{
                        lighting off


                        ambient 1 1 1 1
			diffuse 1 1 1 1
			specular 0 0 0 0
			emissive 0 0 0


			alpha_rejection greater 128
			depth_write on

			texture_unit
			{
				texture NUESTRO_ARCHIVO_DE_IMAGEN
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

Latas

Lata de cerveza

Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cerveza.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido: 

<?xml version="1.0"?>

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

<link name="world" />

<joint name="lata" 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.0325" length="0.1115" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <material name="red">
      <color rgba="1 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <geometry>
      <cylinder radius="0.0325" length="0.1115" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <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>

<joint name="lata_top" type="fixed">
  <parent link="lata_link"/>
  <child link="lata_top_link"/>
  <origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

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

<link name="lata_bottom_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.007" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.007" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

<gazebo reference="lata_link">
  <material>material_fer/Amstel</material>
  <mu1>100</mu1>
  <mu2>100</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_top_link">
  <material>PR2/Grey0</material>
  <mu1>100</mu1>
  <mu2>100</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_bottom_link">
  <material>PR2/Grey0</material>
  <mu1>100</mu1>
  <mu2>100</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

</robot>

Ahora debemos añadir el material "Amstel" a gazebo. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material": 

material material_fer/Amstel
{
	technique
	{
		pass
		{
                        lighting off


                        ambient 1 1 1 1
			diffuse 1 1 1 1
			specular 0 0 0 0
			emissive 0 0 0


			alpha_rejection greater 128
			depth_write on

			texture_unit
			{
				texture amstel.png
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en gazebo:

Textura lata de cerveza
Lata de cerveza en gazebo
Lata de cola

Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cola.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido: 

<?xml version="1.0"?>

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

<link name="world" />

<joint name="lata" 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.0325" length="0.1115" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <material name="red">
      <color rgba="1 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <geometry>
      <cylinder radius="0.0325" length="0.1115" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <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>

<joint name="lata_top" type="fixed">
  <parent link="lata_link"/>
  <child link="lata_top_link"/>
  <origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

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

<link name="lata_bottom_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.007" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.007" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

<gazebo reference="lata_link">
  <material>material_fer/CocaCola</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_top_link">
  <material>PR2/Grey0</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_bottom_link">
  <material>PR2/Grey0</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

</robot>

Ahora debemos añadir el material "CocaCola" a gazebo. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material": 

material Gazebo/CocaCola
{
	technique
	{
		pass
		{
                        lighting off


                        ambient 1 1 1 1
			diffuse 1 1 1 1
			specular 0 0 0 0
			emissive 0 0 0


			alpha_rejection greater 128
			depth_write on

			texture_unit
			{
				texture coca_cola.png
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en gazebo:


Textura lata de cola
Lata de cola gazebo

Para poder cargar los modelo en gazebo debemos convertirlos a ".urdf", para extender las macros y calcular campos, de nuestros archivos ".xacro". Para esto simplemente deberemos ejecutar la siguiente secuencia de comandos en un terminal: 

roscd objetos_fer_modelos
cd urdf
rosrun xacro xacro.py OBJETO.xacro > OBJETO.urdf

Ejecución

Para la carga de todos los modelos se ha creado un launcher, donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_latas.launch" dentro del directorio "launch" del package "myrabot_fer", con el sigiente contenido: 

<launch>

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

  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
  
  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />    
  
  <node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x 0.38 -y -0.18 -z 0.74 -Y -1.57 -model lata_cocacola" respawn="false" output="screen" />  

  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.38 -y 0.17 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />  

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

</launch>

Para iniciar el launcher simplemente debemos ejecutar el siguiente comando en un terminal: 

roslaunch myrabot_fer myrabot_latas.launch

En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa escoja, en el que el brazo señala el objeto que le indiquemos y pasado un breve período de tiempo regresa a su posición inicial. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal: 

roslaunch brazo_fer escoja.launch

Para seleccionar el objeto a señalar publicaremos en el topic "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando: 

rostopic pub selected_object std_msgs/Int16 1 --once

Si todo va bien el resultado será similar al que se muestra en el siguiente vídeo:

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

Ejecución del programa seguir objetos

Programa para mover objetos en gazebo usando el teclado

Para poder desplazar los modelos de los objetos dentro de gazebo hemos escrito un pequeño programa que publica en el topic "gazebo/set_model_state" la posición del modelo del objeto indicado, en este caso "Lata_amstel", usando el teclado. Crearemos un archivo llamado "mover_objetos.cpp" que guardaremos en el directorio "src" del package que hemos creado anteriormente para contener los modelos de los objetos, con el siguiente contenido: 

  #include "ros/ros.h"
  #include "gazebo/ModelState.h"
  #include <signal.h>
  #include <termios.h>
  #include <stdio.h> 
 
  #define PI 3.14159265
  #define L1 104
  #define L2 104
  #define Lp 60
  
  #define KEYCODE_Xmas 0x43 
  #define KEYCODE_Xmenos 0x44
  #define KEYCODE_Ymas 0x41
  #define KEYCODE_Ymenos 0x42
  #define KEYCODE_Zmas 0x77
  #define KEYCODE_Zmenos 0x73
  #define KEYCODE_XgiroMas 0x61
  #define KEYCODE_XgiroMenos 0x64
  #define KEYCODE_YgiroMas 0x65
  #define KEYCODE_YgiroMenos 0x71  
  
struct termios cooked, raw; 

int cont = 0;

double retardo = 0.1;

gazebo::ModelState objeto;  
 
void quit(int sig)
{
  tcsetattr(0, TCSANOW, &cooked);
  ros::shutdown();
  exit(0);
}

void callback(const ros::TimerEvent&)
{
 
	ros::NodeHandle n;   	
  	
  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);

        std::string modelo_objeto;

        ros::NodeHandle nh("~");

        nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));

	ros::Time anterior;

	if (::cont == 0)
        {
		anterior = ros::Time::now();
		
		::objeto.model_name = modelo_objeto;
  
		::objeto.reference_frame = "world";
		
		::objeto.pose.position.x = 0;
		
		::objeto.pose.position.y = 0.5;
		
		::objeto.pose.position.z = 0.74;
		
		::objeto.pose.orientation.x = 0;
		
		::objeto.pose.orientation.y = 0;
		
		::objeto.pose.orientation.z = 0;
		
		::objeto.pose.orientation.w = 1;												 
		
		::cont = 1;

		object_pub_.publish(::objeto);		
		
	}
	
  char c;

  // get the console in raw mode                                                              
  tcgetattr(0, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(0, TCSANOW, &raw);

  puts(""); 
  puts("#####################################################");
  puts("                   CONTROLES OBJETO");
  puts("#####################################################");
  puts("");  
  puts("Flecha arriba:_______ Subir objeto");
  puts("Flecha abajo:________ Bajar objeto");
  puts("Flecha izquierda:____ Desplazar objeto a la izquierda"); 
  puts("Flecha derecha:______ Desplazar objeto a la derecha");
  puts("Tecla W:_____________ Desplazar objeto hacia delante");
  puts("Tecla S:_____________ Desplazar objeto hacia atrás");
  puts("Tecla A:_____________ Inclinar objeto en eje x mas");
  puts("Tecla D:_____________ Inclinar objeto en eje x menos");
  puts("Tecla Q:_____________ Inclinar objeto en eje y mas");
  puts("Tecla E:_____________ Inclinar objeto en eje y menos");                               

        
    while (ros::ok())
    {
		
			
    // get the next event from the keyboard  
    if(read(0, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }
	
      if (c == KEYCODE_Xmas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.x = ::objeto.pose.position.x + 0.005;
			
			anterior = ros::Time::now();
		}		
      }
      if (c == KEYCODE_Xmenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.x = ::objeto.pose.position.x - 0.005;

			anterior = ros::Time::now();
		}				
      }
      if (c == KEYCODE_Ymas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;
			
			anterior = ros::Time::now();
		}					    
      }
      if (c == KEYCODE_Ymenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;
			
			anterior = ros::Time::now();
		}						
      }
      if (c == KEYCODE_Zmas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;
			
			anterior = ros::Time::now();
		}					
      }
      if (c == KEYCODE_Zmenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;
			
			anterior = ros::Time::now();
		}				
      }
      if (c == KEYCODE_XgiroMas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;
			
			anterior = ros::Time::now();
		}	
      }
      if (c == KEYCODE_XgiroMenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.x = ::objeto.pose.orientation.x - 0.1;
			
			anterior = ros::Time::now();
		}	
      }
      if (c == KEYCODE_YgiroMas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.y = ::objeto.pose.orientation.y + 0.1;
			
			anterior = ros::Time::now();
		}	
      }
      if (c == KEYCODE_YgiroMenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.y = ::objeto.pose.orientation.y - 0.1;

			anterior = ros::Time::now();
		}	
      }
      
      object_pub_.publish(::objeto);

	}	
}
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "mover_objetos");   
 
	ros::NodeHandle n;
  	
  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);    	 	


	signal(SIGINT,quit); 
	
	ros::Timer timer = n.createTimer(ros::Duration(1), callback);
    
   	   
 
	ros::spin();  
 
        return 0;
  }

Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro package: 

rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)

Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal: 

roscd objetos_fer_modelos
make

Ejecución

Para la carga de todos los modelos se ha creado un launcher, donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_lata.launch" dentro del directorio "launch" del package "myrabot_fer", con el sigiente contenido: 

<launch>

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

  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
  
  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />     

  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.5 -y 0 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />  

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

</launch>

Para iniciar el launcher simplemente debemos ejecutar el siguiente comando en un terminal: 

roslaunch myrabot_fer myrabot_lata.launch

En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa seguir objetos, en el que el brazo sigue el objeto que le indiquemos. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal: 

roslaunch brazo_fer seguir.launch

Para seleccionar el objeto a señalar publicaremos en el topic "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando: 

rostopic pub selected_object std_msgs/Int16 1 --once

Para poder mover la lata, ejecutaremos en un nuevo terminal el siguiente comando, donde indicamos en el parámetro "modelo_objeto" el nombre del modelo en gazebo que queremos mover: 

rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel

Si todo va bien el resultado será similar al que se muestra en el vídeo siguiente:

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

-->

< go back to main

Retrieved from "https://robotica.unileon.es/index.php?title=MYRAbot_model_for_simulation_(urdf%2bgazebo)&oldid=3061"