Integración de MYRAbot en moveIt! (gazebo+moveIt!)
From robotica.unileon.es
Pasos previos
Instalación de moveIt!
MoveIt! es un package que proporciona una herramienta software para todo tipo de tareas de manipulación tanto a nivel industrial como doméstico. Está disponible para las distribuciones groovy e hydro de ROS, por lo que tendremos que tener instalada alguna de ellas. Para instalar moveIt! ejecutaremos el siguiente comando en un terminal, donde "ROS_DISTRIBUCION" corresponde con la distribución que tenemos instalada:
sudo apt-get install ros-ROS_DISTRIBUCION-moveit-fullModificación del modelo de MYRAbot para moveIt!
Nuevo URDF del brazo
Necesitamos modificar el tipo de controladores, de las articulaciones del brazo, empleados en gazebo para permitir que pueda tomar el control de estos moveIt!. También se han añadido al modelo del brazo las mallas 3D de los componentes reales. Crearemos un archivo llamado "brazo-macros_moveit.xacro" dentro del directorio "urdf" del package "brazo_fer_modelo" con el siguiente contenido:
<?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>
Los archivos empleados, con los modelos de malla 3D ".stl", en la descripción del brazo se pueden descargar a continuación, deben guardarse en el directorio "meshes" del package "brazo_fer_modelo":
Crearemos un archivo llamado "brazo_moveit.xacro" dentro del directorio "urdf" del package "brazo_fer_modelo" con el siguiente contenido:
<?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>
Controladores para simulación brazo
Crearemos un archivo llamado "controller_moveit.yaml" dentro del directorio "config" del package "brazo_fer_modelo" con el siguiente contenido:
# 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}Nuevo URDF soporte y cámara xtion
Se ha colocado una estructra de soporte para situar la nueva cámara asus Xtion PRO LIVE para poder realizar tareas de manipulación y reconocimiento de objetos, en sustitución de la anterior 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":
...
<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" />
...
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:
<?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>
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:
Nuevo URDF de MYRAbot
También debemos crear un archivo llamado "myrabot_moveit.urdf.xacro" dentro del directorio "urdf" del package "myrabot_fer_modelo" con el siguiente contenido, para cargar el modelo del robot con el nuevo modelo del brazo:
<?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>
Crearemos un archivo llamado "myrabot_gazebo_moveit.launcher" dentro del directorio "launch" del package "myrabot_fer_modelo" con el siguiente contenido, para cargar en gazebo el nuevo modelo del robot adaptado a 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>
Configuración de MYRAbot en moveIt!
Asistente de configuración
Una vez instalado moveIt! ejecutaremos el siguiente comando en un terminal para iniciar el asistente de configuración de este package:
roslaunch moveit_setup_assistant setup_assistant.launchNos aparecerá la ventana que se muestra a continuación, donde podemos escoger entre crear un nuevo package de configuración (Create New MoveIt Configuration Package) o editar un package de configuración existente (Edit existing MoveIt Configuration Package):
 Ventana de inicio asistente moveIt! |
Seleccionamos el archivo URDF del modelo de MYRAbot pinchando en "Browse" > |
 Ventana creación nuevo package asistente moveIt! |
Cuando tengamos seleccionado el modelo del robot pincharemos en "Load Files" para que el asistente de moveIt! cargue nuestro modelo. Al finalizar el proceso de carga aparecerá el modelo del robot en la parte derecha de la ventana, como se puede ver en la siguiente imagen:
Ventana carga modelo robot asistente moveIt!
Ahora seleccionaremos en el menú de la parte izquierda de la ventana "Self-Collision" y pincharemos en "Regenerate Default Collision Mtrix" para calcular la matriz de colisiones de nuestro modelo, podemos ver el resultado en la siguiente imagen:
Ventana Self-Collision asistente moveIt!
En el menú de la izquierda seleccionaremos "Virtual Joints" y pincharemos en "Add Virtual Joint". En el campo "Virtual Joint Name" escribiremos virtual_joint, en el campo "Child_Link" seleccionaremos el link base_footprint, en el campo "Parent Frame Name" escribiremos odom y en el campo "Joint Type" seleccionaremos planar, como se muestra en la siguiente imagen:
 Ventana añadir "Virtual Joint" asistente moveIt! |
 Ventana "Virtual Joints" asistente moveIt! |
En el menú de la izquierda seleccionaremos "Planning Groups" y pincharemos en "Add group". En el campo "Group Name" escribiremos brazo y en el campo "Kinematic Solver" seleccionaremos kdl_kinematics_plugin/KDLKinematicsPlugin, el resto de campos se dejarán como viene por defecto como se muestra en la siguiente imagen:
 Ventana añadir "Group" asistente moveIt! |
Seleccionamos de la lista las articulaciones del brazo (base, arti1, arti2, arti3) > |
 Ventana añadir "Joints" a "Group" asistente moveIt! |
 Ventana "Planning Group" asistente moveIt! |
También debemos añadir el grupo de la pinza pinchando en "Add group". En el campo "Group Name" escribiremos pinza y en el campo "Kinematic Solver" seleccionaremos None, el resto de campos se dejarán como viene por defecto como se muestra en la siguiente imagen:
 Ventana añadir "Group" asistente moveIt! |
Seleccionamos de la lista las partes de la pinza > |
 Ventana añadir "Links" a "Group" asistente moveIt! |
 Ventana "Planning Group" asistente moveIt! |
Podemos establecer posiciones predefinidas para el brazo y así poder reutilizarlas. En el menú de la izquierda seleccionaremos "Robot Poses" y pincharemos en "Add Pose". En el campo "Pose Name" escribiremos home y en el campo "Planning Group" seleccionaremos brazo. Con los cursores podemos modificar la posición de cada articulación para establecer la posición deseada, como se muestra en la siguiente imagen:
 Ventana añadir "Robot Pose" asistente moveIt! |
 Ventana "Robot Pose" asistente moveIt! |
En el menú de la izquierda seleccionaremos "End Effectors" y pincharemos en "Add End Effector". En el campo "End Effector Name" escribiremos pinza_eef, en el campo "End Effector Group" seleccionaremos pinza, en el campo "Parent Link" seleccionaremos servo_arti3_link y el campo "Parent Group" lo dejaremos en blanco, como se ,muestra en la imagen:
 Ventana añadir "End Effector" asistente moveIt! |
 Ventana "End Effectors" asistente moveIt! |
Para finalizar seleccionamos "Configuration Files" en el menú de la izquierda y pincharemos en "Browse" para seleccionar la ubicación y nombre del package de configuración. Para generar el package simplemente pincharemos en "Generate Package", como se muestra en la imagen:
Ventana final asistente moveIt!
Integración de controladores del brazo
Crearemos un archivo llamado "controllers.yaml" dentro del directorio "config" del package de configuración de moveIt! creado con el siguiente contenido:
controller_list:
- name: brazo_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- arti1
- arti2
- arti3
- baseEditaremos el archivo "MYRAbot_moveit_controller_manager.launch.xml" situado en el directorio "launch" del package de configuración de moveIt! creado para que su contenido sea el siguiente:
<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- load controller_list -->
<rosparam file="$(find myrabot_moveit_generated)/config/controllers.yaml"/>
</launch>
Integración de sensores de percepción
Dentro de estos sensores tenemos la asus xtion pro live y la kinect, con las cuales puede estar equipado MYRAbot y las cuales se configuran de igual modo. Crearemos un archivo llamado "xtion.yaml" dentro del directorio "config" del package de configuración de moveIt! creado con el siguiente contenido:
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /xtion/depth/points
max_range: 5.0
point_subsample: 1
padding_offset: 0.1
padding_scale: 1.0
filtered_cloud_topic: filtered_cloudEditaremos el archivo "MYRAbot_moveit_sensor_manager.launch.xml" situado en el directorio "launch" del package de configuración de moveIt! creado para que su contenido sea el siguiente:
<launch>
<rosparam command="load" file="$(find myrabot_moveit_generated)/config/xtion.yaml" />
<param name="octomap_frame" type="string" value="odom" />
<param name="octomap_resolution" type="double" value="0.05" />
<param name="max_range" type="double" value="5.0" />
</launch>
Tenemos que añadir el valor move_group/MoveGroupGetPlanningSceneService al parámetro "capabilities" en el archivo "move_group.launch" que se encuantra dentro del directorio "launch" del package de configuración de moveIt! creado, quedando como se muestra a continuación:
...
<!-- MoveGroup capabilities to load -->
<param name="capabilities" value="move_group/MoveGroupCartesianPathService
move_group/MoveGroupExecuteService
move_group/MoveGroupKinematicsService
move_group/MoveGroupMoveAction
move_group/MoveGroupPickPlaceAction
move_group/MoveGroupPlanService
move_group/MoveGroupQueryPlannersService
move_group/MoveGroupStateValidationService
move_group/MoveGroupGetPlanningSceneService
" />
...
