Difference between revisions of "Integration of MYRAbot in moveIt! (gazebo+moveIt!)"

Revision as of 16:21, 11 March 2014

Previous steps

Installation of moveIt!

MoveIt! is a package that provides a software tool for all types of manipulation tasks both industrial level and domestic level. It is available for the ROS distributions groovy and hydro, for this reason we must have installed some of these. We will execute the next command in a terminal in order to install moveIt!, where "ROS_DISTRIBUCION" is our installed distribution:

sudo apt-get install ros-ROS_DISTRIBUCION-moveit-full

Modification of MYRAbot model for moveIt!

New URDF of the arm

We need to modify the type of the gazebo controllers of the arm's joints in order to allow to moveIt! to take the control of these. Also we have added the 3D meshes of the real components of the arm. We will create a file named "brazo-macros_moveit.xacro" within the folder "urdf" of the package "brazo_fer_modelo" with the content that is shown below:

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

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

We will create a file named "brazo_moveit.xacro" within the folder "urdf" of the package "brazo_fer_modelo" with the content that is shown below:

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

Controllers for arm simulation

We will create a file named "controller_moveit.yaml" within the folder "config" of the package "brazo_fer_modelo" with the content that is shown below:

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

New URDF for xtion camera and frame

We have added a frame to MYRAbot in order to place the new asus Xtion PRO LIVE camera which replaces the previous webcam. We will add the next code lines to the file "estructura-myrabot.xacro" placed within the folder "urdf" of the package "myrabot_fer_modelo" in order to add the camera frame:

...

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

...

We will create a file named "xtion.xacro" within the folder "urdf" of the package "myrabot_fer_modelo" with the content that is shown below in order to create the model of the 3D sensor:

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

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

New URDF of MYRAbot

As well we have to create a file named "myrabot_moveit.urdf.xacro" within the folder "urdf" of the package "myrabot_fer_modelo" with the content that is show below in order to add to the MYRAbot model the new arm model:

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

We will create a file named "myrabot_gazebo_moveit.launcher" within the folder "launch" of the package "myrabot_fer_modelo" with the content that is shown below in order to load in gazebo the new robot model adapted to moveIt!:

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

Configuration of MYRAbot in moveIt!

Setup assistant

< go back to main

@@ Line 580: / Line 580: @@
 </syntaxhighlight>
+====New URDF for xtion camera and frame====
+We have added a frame to MYRAbot in order to place the new [http://www.asus.com/Multimedia/Xtion_PRO_LIVE/ asus Xtion PRO LIVE] camera which replaces the previous [[Objects recognition and position calculation (webcam)|webcam]]. We will add the next code lines to the file "estructura-myrabot.xacro" placed within the folder "urdf" of the ''package'' "myrabot_fer_modelo" in order to add the camera frame:
-<!--
-====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">
@@ Line 627: / Line 623: @@
 </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:
+We will create a file named "xtion.xacro" within the folder "urdf" of the ''package'' "myrabot_fer_modelo" with the content that is shown below in order to create the model of the 3D sensor:
 <syntaxhighlight lang="xml" enclose="div">
@@ Line 634: / Line 630: @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-   Xacro properties
+  <!-- Xacro properties -->
    <xacro:property name="M_SCALE" value="0.001"/>
    <xacro:property name="asus_xtion_pro_depth_rel_rgb_py" value="0.0270" />
@@ Line 641: / Line 637: @@
    <property name="M_PI" value="3.14159"/>
-   Parameterised in part by the values in turtlebot_properties.urdf.xacro
+  <!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
    <xacro:macro name="xtion" params="parent *origin">
@@ Line 778: / Line 774: @@
 </syntaxhighlight>
+We will add the next files within the folder "meshes" of the ''package'' "myrabot_fer_modelo":
-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]
@@ Line 785: / Line 780: @@
 * [http://www.fernando.casadogarcia.es/images/asus_xtion_pro_live.png asus_xtion_pro_live.png]
-====Nuevo URDF de MYRAbot====
+====New URDF of 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 cargar el [[Modelo para simulación MYRAbot (urdf+gazebo)|modelo del robot]] con el nuevo [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|modelo del brazo]]:
+As well we have to create a file named "myrabot_moveit.urdf.xacro" within the folder "urdf" of the ''package'' "myrabot_fer_modelo" with the content that is show below in order to add to the [[MYRAbot model for simulation (urdf+gazebo)|MYRAbot model]] the new [[MYRAbot's arm model for simulation (urdf+gazebo)|arm model]]:
 <syntaxhighlight lang="xml" enclose="div">
@@ Line 862: / Line 857: @@
 </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!]:
+We will create a file named "myrabot_gazebo_moveit.launcher" within the folder "launch" of the ''package'' "myrabot_fer_modelo" with the content that is shown below in order to load in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] the new robot model adapted to [http://moveit.ros.org/ moveIt!]:
 <syntaxhighlight lang="xml" enclose="div">
@@ Line 906: / Line 901: @@
    <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
+    <!-- heights are in the (optical?) frame of the kinect -->
      <param name="min_height" value="-0.15"/>
      <param name="max_height" value="0.15"/>
@@ Line 914: / Line 909: @@
    <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
+    <!-- heights are in the (optical?) frame of the kinect -->
      <param name="min_height" value="-0.025"/>
      <param name="max_height" value="0.025"/>
@@ Line 925: / Line 920: @@
 </syntaxhighlight>
-==Configuración de MYRAbot en moveIt!==
+==Configuration of MYRAbot in moveIt!==
+===Setup assistant===
+<!--
-===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'':

Difference between revisions of "Integration of MYRAbot in moveIt! (gazebo+moveIt!)"

Revision as of 16:21, 11 March 2014

Contents

Previous steps

Installation of moveIt!

Modification of MYRAbot model for moveIt!

New URDF of the arm

Controllers for arm simulation

New URDF for xtion camera and frame

New URDF of MYRAbot

Configuration of MYRAbot in moveIt!

Setup assistant

Navigation menu

Personal tools

Namespaces

Variants

Views

More

Search

Resources

Activities

Tools