<?xml version="1.0"?>
<robot name="man_quad"

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

<!--*************************************QUADROTOR*************************************-->

<!-- CUERPO BASE -->
  
  <link name="Base">  <!-- Declaracion de un cuerpo o eslabon del robot-->
    <visual>
       <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_base_escala.dae" scale="0.0255 0.0255 0.0255"/> 
       </geometry>       
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_base_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 5.9e-3" rpy="0 0 0"/>
      <mass value="1.578"/>
      <inertia ixx="0.01632655" ixy="0.063e-7" ixz="0.124e-7" iyy="0.01632655" iyz="0.113e-7" izz="0.02642555"/>
    </inertial>

  </link>
  
  <!-- BASE MOTORES -->
    
  <link name="motor_1"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.020" rpy="0 0 0"/>
      <mass value="0.033"/>
      <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
    </inertial>
  </link>

  <joint name="base_a_motor_1" type="fixed"> 
    <parent link="Base"/>
    <child link="motor_1"/>
    <origin rpy="0 0 0" xyz="0.2873 0 0.015"/>
  </joint>
  
  <link name="motor_4"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.020" rpy="0 0 0"/>
      <mass value="0.033"/>
      <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
    </inertial>
  </link>

  <joint name="base_a_motor_4" type="fixed"> 
    <parent link="Base"/>
    <child link="motor_4"/>
    <origin rpy="0 0 1.57079" xyz="0 0.2873 0.015"/>
  </joint>
   
  <link name="motor_3"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.020" rpy="0 0 0"/>
      <mass value="0.033"/>
      <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
    </inertial>
  </link>

  <joint name="base_a_motor_3" type="fixed"> 
    <parent link="Base"/>
    <child link="motor_3"/>
    <origin rpy="0 0 3.14159" xyz="-0.2873 0 0.015"/>
  </joint>
  
  <link name="motor_2"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_motor_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.020" rpy="0 0 0"/>
      <mass value="0.033"/>
      <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
    </inertial>
  </link>

   <joint name="base_a_motor_2" type="fixed"> 
    <parent link="Base"/>
    <child link="motor_2"/>
    <origin rpy="0 0 4.71239" xyz="0 -0.2873 0.015"/>
  </joint>

<!-- HELICES -->

<link name="helice_1"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala_amarillo.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala_amarillo.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.022" rpy="0 0 0"/>
      <mass value="0.005"/>
      <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
    </inertial>
  </link>

<!--
  <joint name="motor_a_helice_1" type="continuous"> 
-->
  <joint name="motor_a_helice_1" type="fixed">  <!--Realmnte seria continua pero asi tengo problemas de estabilidad, y si lo pongo como fixed no me aparece el frame del link en gazebo-->
    <parent link="motor_1"/>
    <child link="helice_1"/>
    <origin rpy="0 0 0" xyz="0 0 0.04"/>
<!--
    <axis xyz="0 0 1"/>
    <limit lower="0.0" upper="0.00000000000000000000001" velocity="1000" effort="1000"/> Para que me aperezca el frame en gazebo, aprox a fixed
-->
  </joint>
 
 <link name="helice_4"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.022" rpy="0 0 0"/>
      <mass value="0.005"/>
      <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
    </inertial>
  </link>

<!--
  <joint name="motor_a_helice_4" type="continuous"> 
-->
  <joint name="motor_a_helice_4" type="fixed"> 
    <parent link="motor_4"/>
    <child link="helice_4"/>
    <origin rpy="0 0 0" xyz="0 0 0.04"/>
<!--
    <axis xyz="0 0 1"/>
    <limit lower="0.0" upper="0.00000000000000000000001" velocity="1000" effort="1000"/> Para que me aperezca el frame en gazebo, aprox a fixed-
-->
  </joint>

<link name="helice_3"> 
    <visual>
      <geometry>
        <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.022" rpy="0 0 0"/>
      <mass value="0.005"/>
      <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
    </inertial>
  </link>

<!--
  <joint name="motor_a_helice_3" type="continuous"> 
-->
  <joint name="motor_a_helice_3" type="fixed"> 
    <parent link="motor_3"/>
    <child link="helice_3"/>
    <origin rpy="0 0 0" xyz="0 0 0.04"/>
<!-- 
    <axis xyz="0 0 1"/>
    <limit lower="0.0" upper="0.00000000000000000000001" velocity="1000" effort="1000"/> Para que me aperezca el frame en gazebo, aprox a fixed-->
  </joint>
  
  <link name="helice_2"> 
    <visual>
      <geometry>
       <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala.dae" scale="0.0255 0.0255 0.0255"/>     
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
         <mesh filename="package://Quadrotor_manipulador/visual/quadrotor/pelican_helice_escala.dae" scale="0.0255 0.0255 0.0255"/> 
      </geometry>
    </collision>
    <inertial>
	  <origin xyz="0 0 0.022" rpy="0 0 0"/>
      <mass value="0.005"/>
      <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
    </inertial>
  </link>

<!--
  <joint name="motor_a_helice_2" type="continuous"> 
-->
  <joint name="motor_a_helice_2" type="fixed"> 
    <parent link="motor_2"/>
    <child link="helice_2"/>
    <origin rpy="0 0 0" xyz="0 0 0.04"/>
<!--
    <axis xyz="0 0 1"/>
    <limit lower="0.0" upper="0.00000000000000000000001" velocity="1000" effort="1000"/> Para que me aperezca el frame en gazebo, aprox a fixed  
-->
  </joint>
  
  
  
   <!--GAZEBO PLUGING-->
  <gazebo>  
		<!--SENSOR DE ODOMETRIA-->
		<controller:gazebo_ros_p3d name="sensor_odometria" plugin="libgazebo_ros_p3d.so"> <!--Uso plugin gazebo_ros_p3d del paquete gazebo_plugins-->
			<alwaysOn>true</alwaysOn>  <!--ON/OFF-->
			<updateRate>1000.0</updateRate>  <!--Frecuencia del topic-->
			<bodyName>Base</bodyName>  <!--Cuerpo sobre el que se monta-->
			<frameName>plane</frameName> <!--Frame de referencia sobre el que se toman las medidas-->
			<topicName>Odometria</topicName> <!-- Topic por el que se envian los datos de Odometria: Pose y Twist-->
			<gaussianNoise>0.0</gaussianNoise>  <!--Ruido en las medidas-->    
		</controller:gazebo_ros_p3d>
		
  </gazebo>




<!--************************SOPORTES************************************-->

  <link name="Soporte_1"> 
		<visual>
		   <geometry>
			 <cylinder length="0.5" radius="0.005"/>
		   </geometry> 
	       <material name="blue">
               <color rgba="0 0.5 0.5 1"/>
           </material>      
		</visual>
		<collision>
		  <geometry>
			 <cylinder length="0.5" radius="0.005"/> 
		  </geometry>
		</collision>
		<inertial>
		  <origin xyz="0 0 0" rpy="0 0 0"/>
		  <mass value="0"/>
		  <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
		</inertial>
  </link>
  
  <joint name="base_a_Soporte_1" type="fixed" >
    <parent link="Base" />
    <child link="Soporte_1" />
    <origin rpy="0 0 0" xyz="0.2873 0 -0.242"/>
  </joint> 
  
   <gazebo reference="Soporte_1">
    <material>Gazebo/Blue</material>
  </gazebo>
  
    <link name="Soporte_2"> 
		<visual>
		   <geometry>
			 <cylinder length="0.5" radius="0.005"/>
		   </geometry> 
	       <material name="blue">
               <color rgba="0 0.5 0.5 1"/>
           </material>      
		</visual>
		<collision>
		  <geometry>
			 <cylinder length="0.5" radius="0.005"/> 
		  </geometry>
		</collision>
		<inertial>
		  <origin xyz="0 0 0" rpy="0 0 0"/>
		  <mass value="0"/>
		  <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
		</inertial>
  </link>
  
  <joint name="base_a_Soporte_2" type="fixed" >
    <parent link="Base" />
    <child link="Soporte_2" />
    <origin rpy="0 0 0" xyz="0 0.2873 -0.242"/>
  </joint> 
  
  <gazebo reference="Soporte_2">
    <material>Gazebo/Blue</material>
  </gazebo>
  
    <link name="Soporte_3"> 
		<visual>
		   <geometry>
			 <cylinder length="0.5" radius="0.005"/>
		   </geometry> 
	       <material name="blue">
               <color rgba="0 0.5 0.5 1"/>
           </material>      
		</visual>
		<collision>
		  <geometry>
			 <cylinder length="0.5" radius="0.005"/> 
		  </geometry>
		</collision>
		<inertial>
		  <origin xyz="0 0 0" rpy="0 0 0"/>
		  <mass value="0"/>
		  <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
		</inertial>
  </link>
  
  <joint name="base_a_Soporte_3" type="fixed" >
    <parent link="Base" />
    <child link="Soporte_3" />
    <origin rpy="0 0 0" xyz="-0.2873 0 -0.242"/>
  </joint> 
  
  <gazebo reference="Soporte_3">
    <material>Gazebo/Blue</material>
  </gazebo>
  
    <link name="Soporte_4"> 
		<visual>
		   <geometry>
			 <cylinder length="0.5" radius="0.005"/>
		   </geometry> 
	       <material name="blue">
               <color rgba="0 0.5 0.5 1"/>
           </material>      
		</visual>
		<collision>
		  <geometry>
			 <cylinder length="0.5" radius="0.005"/> 
		  </geometry>
		</collision>
		<inertial>
		  <origin xyz="0 0 0" rpy="0 0 0"/>
		  <mass value="0"/>
		  <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
		</inertial>
  </link>
  
  <joint name="base_a_Soporte_4" type="fixed" >
    <parent link="Base" />
    <child link="Soporte_4" />
    <origin rpy="0 0 0" xyz="0 -0.2873 -0.242"/>
  </joint> 

  <gazebo reference="Soporte_4">
    <material>Gazebo/Blue</material>
  </gazebo>





<!--************************Conexion manipulador al quadrotor*************************** -->

  <joint name="BaseQuad_a_ConjuntoBase" type="fixed" >
    <parent link="Base" />
    <origin xyz="1.51e-3 1.806e-3  -0.077" rpy="0 0 -0.7853981634" />
    <child link="Conjunto_base" />
  </joint> 


<!--*************************************MANIPULADOR*************************************-->

<!--ESLABONES--> 
 		 
		 <link name="Conjunto_base">  <!-- Declaracion de un cuerpo o eslabon del robot-->
			<visual>
			   <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/Conjunto_base_completo.dae" scale="0.0255 0.0255 0.0255"/> 
			   </geometry>       
			</visual>
			<collision>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/Conjunto_base_completo.dae" scale="0.0255 0.0255 0.0255"/> 
			  </geometry>
			</collision>
			<inertial>
			  <origin xyz="3.937e-3 -23.544e-3 -40.343e-3" rpy="0 0 0"/>
			  <mass value="0.214623"/>
			  <inertia ixx="3618.2982e-7" ixy="-549.4840e-7" ixz="-122.1871e-7" iyy="1151.3618e-7" iyz="470.4171e-7" izz="3685.7680e-7"/>
			</inertial>
		  </link>
		  
		  <link name="Bateria"> 
			<visual>
			   <geometry>
				 <box size="0.043 0.150 0.018"/> 
			   </geometry> 
     		   <material name="amarillo">
                 <color rgba="1 1 0 1"/>
               </material>  
			</visual>
			<collision>
			  <geometry>
				 <box size="0.043 0.150 0.018"/> 
			  </geometry>    
			</collision>
			<inertial>
			  <origin xyz="0 0 0" rpy="0 0 0"/>
			  <mass value="0.231"/>
			  <inertia ixx="4416.44e-7" ixy="0.0" ixz="0.0" iyy="420.475e-7" iyz="0.0" izz="4711.531e-7"/>
			</inertial>
		  </link> 
		  <gazebo reference="Bateria">
			<material>Gazebo/Yellow</material>
		  </gazebo>
		  
		  <link name="Brazo_1"> 
			<visual>
			  <geometry>
			   <mesh filename="package://Quadrotor_manipulador/visual/manipulador/brazo1.dae" scale="0.0255 0.0255 0.0255"/>     
			  </geometry>
			  <origin rpy="0 0 0" xyz="0 0 0"/>
			</visual>
			<collision>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/brazo1.dae" scale="0.0255 0.0255 0.0255"/> 
			  </geometry>
			</collision>
			<inertial>
			  <origin xyz="-5.654e-3 0.393e-3 -102.056e-3" rpy="0 0 0"/>
			  <mass value="0.101602"/>
			  <inertia ixx="7595.151e-7" ixy="-2.2392e-7" ixz="312.682e-7" iyy="8157.2174e-7" iyz="-7.9513e-7" izz="776.2473e-7"/>
			</inertial>
		  </link>
		  
		  
		  <link name="Brazo_2"> 
			<visual>
			  <geometry>
			   <mesh filename="package://Quadrotor_manipulador/visual/manipulador/brazo2.dae" scale="0.0255 0.0255 0.0255"/>     
			  </geometry>
			  <origin rpy="0 0 0" xyz="0 0 0"/>
			</visual>
			<collision>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/brazo2.dae" scale="0.0255 0.0255 0.0255"/> 
			  </geometry>
			</collision>
			<inertial>
			  <origin xyz="-1.883e-3 94.381e-3 1.436e-3" rpy="0 0 0"/>
			  <mass value="0.036285"/>
			  <inertia ixx="1041.690e-7" ixy="-31.590e-7" ixz="1.178e-7" iyy="143.044e-7" iyz="0.430e-7" izz="1126.690e-7"/>
			</inertial>
		  </link>
		  
		   <link name="Chasis_efector"> 
			<visual>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/Chasis_efector.dae" scale="0.0255 0.0255 0.0255"/>     
			  </geometry>
			  <origin rpy="0 0 0" xyz="0 0 0"/>
			</visual>
			<collision>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/Chasis_efector.dae" scale="0.0255 0.0255 0.0255"/> 
			  </geometry>
			</collision>
			<inertial>
			  <origin xyz="-4.527e-3 8.189e-3 -0.900e-3" rpy="0 0 0"/>
			  <mass value="0.016902"/>
			  <inertia ixx="14.33064e-7" ixy="-0.364275e-7" ixz="-0.589219e-7" iyy="21.987118e-7" iyz="-0.146395e-7" izz="15.210762e-7"/>
			</inertial>
		  </link>
		  
		  <link name="Gancho_efector"> 
			<visual>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/Gancho_efector.dae" scale="0.0255 0.0255 0.0255"/>     
			  </geometry>
			  <origin rpy="0 0 0" xyz="0 0 0"/>
			</visual>
			<collision>
			  <geometry>
				 <mesh filename="package://Quadrotor_manipulador/visual/manipulador/Gancho_efector.dae" scale="0.0255 0.0255 0.0255"/> 
			  </geometry>
			</collision>
			<inertial>
			  <origin xyz="0.809e-3 8.634e-3 -3.791e-3" rpy="0 0 0"/>
			  <mass value="0.001593"/>
			  <inertia ixx="1.430595e-7" ixy="0.009745e-7" ixz="-0.008666e-7" iyy="3.17436e-7" iyz="-0.011176e-7" izz="3.715269e-7"/>
			</inertial>
		  </link>
 
		  
		  <link name="fantasma"> 
			<visual>
			  <geometry>
				 <box size="0.1 0.008 0.008"/>     
			  </geometry>
		      <material name="Azulito">
                 <color rgba="0 0 0.5 0.5"/>
              </material>
			  <origin rpy="0 0 0" xyz="0 0 0"/>
			</visual>
			<inertial>
			  <origin xyz="0 0 0" rpy="0 0 0"/>
			  <mass value="1e-99"/>
			  <inertia ixx="1e-99" ixy="0.0" ixz="0.0" iyy="1e-99" iyz="0.0" izz="1e-99"/>
			</inertial>
		  </link>
  
<!--ARTICULACIONES-->
  	  
		  <joint name="Bandeja_bateria" type="fixed"> <!--prismatic--> 
			<parent link="Conjunto_base"/>
			<child link="Bateria"/>
			<origin rpy="0 0 0" xyz="0 -30e-3 -11e-3"/>
<!--
			<axis xyz="0 1 0"/>
-->
<!--
			<limit lower="-0.04" upper="0" velocity="6.98" effort="0.94"/> 
-->
<!--
		    <safety_controller k_velocity="2.04" k_position="10" soft_lower_limit="-1" soft_upper_limit="1.5"/> 
-->
		  </joint>
		  
		  <joint name="Articulacion_1" type="revolute"> 
			<parent link="Conjunto_base"/>
			<child link="Brazo_1"/>
			<origin rpy="0 0 0" xyz="0 26.00e-3 -55.00e-3"/>
			<axis xyz="1 0 0"/>
			<limit lower="-1.08" upper="1.57" velocity="6.98" effort="0.94"/>     <!-- Ojo: La velocidad [rad/s] es sin carga, por lo que puede que deba de ser menor-->
		    <safety_controller k_velocity="2.04" k_position="10" soft_lower_limit="-1" soft_upper_limit="1.5"/> <!-- Condiciones de seguridad. Necesario incluirlas para que se tengan tambien en cuenta los <limit>!!! -->
		  </joint>																		 
		  
		  <joint name="Articulacion_2" type="revolute"> 
			<parent link="Brazo_1"/>
			<child link="Brazo_2"/>
			<origin rpy="0 0 0" xyz="0 0 -200e-3"/>
			<axis xyz="1 0 0"/>
			<limit lower="-2.8916" upper="0.25" velocity="6.16" effort="0.64"/>      <!--Necesario modificar limite inferior mas alla de cero para que la posicion cero sea accesible. Explicacion: El soft upper limit (limite software) debe ser menor o igual que el upper (fisico-hardware), y en el ademas la velocidad es cero debido al safe-control, por lo cual si elegimos los dos en cero como en un principio hace que la posicion exacta cero no sea accesible -->
			<safety_controller k_velocity="2.04" k_position="10" soft_lower_limit="-2.7" soft_upper_limit="0.25" />
		  </joint>
		  
		  
<!--
		  <joint name="Articulacion_2" type="fixed"> 
			<parent link="Brazo_1"/>
			<child link="Brazo_2"/>
			<origin rpy="0 0 0" xyz="0 0 -200e-3"/>
		  </joint>
-->
		  
		  <joint name="Articulacion_3" type="revolute"> 
			<parent link="Brazo_2"/>
			<child link="Chasis_efector"/>
			<origin rpy="0 0 0" xyz="0 155.77e-3 0"/>
			<axis xyz="0 1 0"/>
<!--
			<limit lower="-1.047" upper="1.047" velocity="9.52" effort="0.22"/> No es necesario tanto esfuerzo, ya que practicamente no tiene inercia
-->
			<limit lower="-1.047" upper="1.047" velocity="9.52" effort="0.01"/> <!--Inicialmente de disminuyo effort a 0.001 lo cual provoca que las perturbaciones se notaran muchisimo-->
			<safety_controller k_velocity="0.145" k_position="100" soft_lower_limit="-0.9" soft_upper_limit="0.9"/>         
		  </joint>

<!--
		  <joint name="Articulacion_3" type="fixed"> 
			<parent link="Brazo_2"/>
			<child link="Chasis_efector"/>
			<origin rpy="0 0 0" xyz="0 155.77e-3 0"/>
		  </joint>
-->
		  
		  <joint name="Articulacion_efector" type="revolute"> 
			<parent link="Chasis_efector"/>
			<child link="Gancho_efector"/>
			<origin rpy="0 0 0" xyz="-4.5e-3 9.5e-3 -6.5e-3"/>
			<axis xyz="1 0 0"/>
			<limit lower="-1" upper="0.6" velocity="9.52" effort="0.22"/>
		  </joint>

<!--
		  <joint name="Articulacion_efector" type="fixed"> 
			<parent link="Chasis_efector"/>
			<child link="Gancho_efector"/>
			<origin rpy="0 0 0" xyz="-4.5e-3 9.5e-3 -6.5e-3"/>
		  </joint>
-->

		    <joint name="Articulacion_fantasma" type="continuous"> 
			  <parent link="Brazo_2"/>
			  <child link="fantasma"/>
			  <origin rpy="0 0 0" xyz="0 178e-3 -9e-3"/>
			  <axis xyz="1 0 0"/>
			</joint>
  
<!--TRANSMISIONES-->  
  
	    <!-- Las transmisiones son importantes para unir los Controladores a los joints-->
        <transmission name="Transmision_1" type="pr2_mechanism_model/SimpleTransmission">
                <actuator name="Motor_1" />
                <joint name="Articulacion_1" />
                <mechanicalReduction>1</mechanicalReduction>
        </transmission>
        
        <transmission name="Transmision_2" type="pr2_mechanism_model/SimpleTransmission">
                <actuator name="Motor_2" />
                <joint name="Articulacion_2" />
                <mechanicalReduction>1</mechanicalReduction>
        </transmission>

        <transmission name="Transmision_3" type="pr2_mechanism_model/SimpleTransmission">
                <actuator name="Motor_3" />
                <joint name="Articulacion_3" />
                <mechanicalReduction>1</mechanicalReduction>
        </transmission>
        
        <transmission name="Transmision_efector" type="pr2_mechanism_model/SimpleTransmission">
                <actuator name="Motor_efector" />
                <joint name="Articulacion_efector" />
                <mechanicalReduction>1</mechanicalReduction>
        </transmission>
        
        <transmission name="Transmision_fantasma" type="pr2_mechanism_model/SimpleTransmission">
                <actuator name="Motor_fantasma" />
                <joint name="Articulacion_fantasma" />
                <mechanicalReduction>1</mechanicalReduction>
        </transmission>

 <!--Coeficientes de contacto para Gazebo-->  
 
  <gazebo reference="Chasis_efector">
      <mu1 value="0.1" /> <!--Coeficiente de friccion en la direccion ppal 1-->
      <mu2 value="0.1" /> <!--Coeficiente de friccion en la direccion ppal 2-->
		
	  <!--SE DEJAN LA RIGIDEZ Y EL AMORTIGUAMIENTO POR DEFECTO-->	
		<!--<kp  value="1000000000000000.0" />--> <!--Rigidez de contacto: K-->
		<!--<kd  value="-100" />--> <!--Amortiguamiento de contacto: C-->
  </gazebo> 


<!--PLUGING: SENSOR DE POSICION DEL EFECTOR-->
  
  <gazebo>  
		<controller:gazebo_ros_p3d name="sensor_pos_efector" plugin="libgazebo_ros_p3d.so"> <!--Uso plugin gazebo_ros_p3d del paquete gazebo_plugins-->
			<alwaysOn>true</alwaysOn>  <!--ON/OFF-->
			<updateRate>10000</updateRate>  <!--Frecuencia del topic-->
			<bodyName>fantasma</bodyName>  <!--Cuerpo sobre el que se monta-->
			<frameName>ref_sensor_pos_TCP</frameName> <!--Frame de referencia sobre el que se toman las medidas--> 	
			<topicName>Brazo_1</topicName> <!-- Topic por el que se envian los datos de Odometria: Pose y Twist-->
			<gaussianNoise>0.0</gaussianNoise>  <!--Ruido en las medidas-->    
		</controller:gazebo_ros_p3d>		
  </gazebo>

       
<!-- Tags para usar controladores en Gazebo--> 

  <gazebo>
     
      <controller:gazebo_ros_time name="gazebo_ros_time" plugin="libgazebo_ros_time.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>1000.0</updateRate>
      </controller:gazebo_ros_time>


      <!-- PR2_ACTARRAY -->

      <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>1000.0</updateRate>
      </controller:gazebo_ros_controller_manager>
  </gazebo>    
      
<!-- Sensor esfuerzo articulacion 2 -->
      
  <gazebo>    
    <controller:gazebo_ros_f3d name="sensor_de_esfuerzo_M2" plugin="libgazebo_ros_f3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>     <!--Frecuencia: la misma que en el control del quad para que nos se nos bloquee/llene la cola del topic y siempre tengamos la actual--> 
        <bodyName>Base</bodyName>    <!--Cuerpo sobre el que leemos los esfuerzos-->
        <topicName>Medida_M2</topicName> <!--Nombre del topic por el que gazebo nos enviara las fuerzas y momentos medidos-->
        <frameName>plane</frameName>	 <!--Simplemente es el NOMBRE del plano de referencia, ya que no se puede variar este-->
        <interface:position name="f3d_Brazo_2_position" /> <!--Obligatorio declararle una interface que usara internamente el Player Stage para la configuracion-->
    </controller:gazebo_ros_f3d>
          
        
<!-- Sensor esfuerzo articulacion 1 --> 
        
    <controller:gazebo_ros_f3d name="sensor_de_esfuerzo_M1" plugin="libgazebo_ros_f3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>     <!--Frecuencia: la misma que en el control del quad para que nos se nos bloquee/llene la cola del topic y siempre tengamos la actual--> 
        <bodyName>Brazo_1</bodyName>    <!--Cuerpo sobre el que leemos los esfuerzos-->
        <topicName>Medida_M1</topicName> <!--Nombre del topic por el que gazebo nos enviara las fuerzas y momentos medidos-->
        <frameName>Base</frameName>	 <!--Simplemente es el NOMBRE del plano de referencia, ya que no se puede variar este-->
        <interface:position name="f3d_Brazo_1_position" /> <!--Obligatorio declararle una interface que usara internamente el Player Stage para la configuracion-->
    </controller:gazebo_ros_f3d>
      
<!-- Sensor esfuerzo sobre el Quad --> 
        
    <controller:gazebo_ros_f3d name="sensor_de_esfuerzo_quad" plugin="libgazebo_ros_f3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>     <!--Frecuencia: la misma que en el control del quad para que nos se nos bloquee/llene la cola del topic y siempre tengamos la actual--> 
        <bodyName>Base</bodyName>    <!--Cuerpo sobre el que leemos los esfuerzos-->
        <topicName>Medida_Mquad</topicName> <!--Nombre del topic por el que gazebo nos enviara las fuerzas y momentos medidos-->
        <frameName>Base</frameName>	 <!--Simplemente es el NOMBRE del plano de referencia, ya que no se puede variar este-->
        <interface:position name="f3d_quad_position" /> <!--Obligatorio declararle una interface que usara internamente el Player Stage para la configuracion-->
    </controller:gazebo_ros_f3d>
      
      
      
  </gazebo>
  
</robot>

