MYRAbot's arm model for simulation (urdf+gazebo)
From robotica.unileon.es
Contents
Creation of URDF model
URDF (Unified Robot Description Format) is the format used to define the model of the arm (xml) in order to simulate it in gazebo. It consists of a tree of geometrical elements (links) plugged through joints(joints). This joints can be fixed or mobile, the mobile joints can be rotating, linear or floating.
First, we will create a new package with the necessary dependences for our programs (urdf brazo_fer std_msgs sensor_msgs tf roscpp). We will execute the next commands in a terminal:
cd ~/ros_workspace
roscreate-pkg brazo_fer_modelo urdf brazo_fer std_msgs sensor_msgs tf roscppWe have divided the model in three files, main file, macros file and placement file. For the main file, we will create a file named "brazo.xacro" within the folder "urdf" of the created package with the content that is shown below:
<?xml version="1.0"?>
<robot>
<include filename="$(find brazo_fer_modelo)/urdf/brazo-macros.xacro" />
<macro name="brazo" params="parent *origin">
<joint name="fixed" type="fixed">
<parent link="${parent}"/>
<child link="base_brazo_link"/>
<insert_block name="origin" />
<axis xyz="0 0 1" />
</joint>
<link name="base_brazo_link">
<visual>
<geometry>
<box size="0.032 0.05 0.04"/>
</geometry>
<origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
<geometry>
<box size="0.032 0.05 0.04"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.0135 0.020"/>
<mass value="0.055"/>
<default_inertia_servos />
</inertial>
</link>
<joint name="base" type="revolute">
<parent link="base_brazo_link"/>
<child link="hombro_link"/>
<origin xyz="0 0 0.04" rpy="0 0 0" />
<axis xyz="0 0 1" />
<default_limit />
<default_dynamics />
</joint>
<link name="hombro_link">
<visual>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
<mass value="0.055"/>
<default_inertia_servos />
</inertial>
</link>
<joint name="arti1" type="revolute">
<parent link="hombro_link"/>
<child link="brazo_link"/>
<origin xyz="0 0 0.0385" rpy="0 0 0" />
<axis xyz="1 0 0" />
<default_limit />
<default_dynamics />
</joint>
<!--Brazo-->
<servo nombre="brazo" />
<joint name="servo_arti1_B" type="fixed">
<parent link="brazo_link"/>
<child link="brazo_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<base nombre="brazo" />
<joint name="servo_arti1_D" type="fixed">
<parent link="brazo_link"/>
<child link="brazo_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<soporte nombre="brazo" simetrico="1" lado="I" />
<joint name="servo_arti1_I" type="fixed">
<parent link="brazo_link"/>
<child link="brazo_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<soporte nombre="brazo" simetrico="-1" lado="D" />
<joint name="arti2" type="revolute">
<parent link="brazo_link"/>
<child link="antebrazo_link"/>
<origin xyz="0 0 0.104" rpy="0 0 0" />
<axis xyz="1 0 0" />
<default_limit />
<default_dynamics />
</joint>
<!--Antebrazo-->
<servo nombre="antebrazo" />
<joint name="servo_arti2_B" type="fixed">
<parent link="antebrazo_link"/>
<child link="antebrazo_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<base nombre="antebrazo" />
<joint name="servo_arti2_D" type="fixed">
<parent link="antebrazo_link"/>
<child link="antebrazo_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<soporte nombre="antebrazo" simetrico="1" lado="I" />
<joint name="servo_arti2_I" type="fixed">
<parent link="antebrazo_link"/>
<child link="antebrazo_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<soporte nombre="antebrazo" simetrico="-1" lado="D" />
<joint name="arti3" type="revolute">
<parent link="antebrazo_link"/>
<child link="muneca_link"/>
<origin xyz="0 0 0.104" rpy="0 0 0" />
<axis xyz="1 0 0" />
<default_limit />
<default_dynamics />
</joint>
<!--Muñeca-->
<link name="muneca_link">
<visual>
<geometry>
<box size="0.05 0.04 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
<geometry>
<box size="0.05 0.04 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00625 0 0.06"/>
<mass value="0.055"/>
<default_inertia_servos />
</inertial>
</link>
<joint name="servo_arti3_B" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_B">
<visual>
<geometry>
<box size="0.05325 0.04 0.005"/>
</geometry>
<origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
<geometry>
<box size="0.05325 0.04 0.005"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.004625 0 0.0415"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_arti3_D" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_SI">
<visual>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
<origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.021 0 0.01625"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_arti3_I" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_SD">
<visual>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
<geometry>
<box size="0.002 0.032 0.0555"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.021 0 0.01625"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_arti3_P" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_P"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<!--Pinza fija-->
<link name="muneca_link_P">
<visual>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
<origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.03425 0 0.090"/>
<mass value="0.001"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_arti3_PS" type="fixed">
<parent link="muneca_link"/>
<child link="muneca_link_PS"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="muneca_link_PS">
<visual>
<geometry>
<box size="0.002 0.04 0.037"/>
</geometry>
<origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>
<geometry>
<box size="0.002 0.04 0.037"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.03225 0 0.0575"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
<joint name="pinza" type="revolute">
<parent link="muneca_link"/>
<child link="pinza_link"/>
<origin xyz="-0.00725 0 0.06" rpy="0 0 0" />
<axis xyz="0 1 0" />
<default_limit />
<default_dynamics />
</joint>
<!--Pinza móvil-->
<link name="pinza_link">
<visual>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
<geometry>
<box size="0.002 0.04 0.092"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.027 0 0.030"/>
<mass value="0.001"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_pinza_B" type="fixed">
<parent link="pinza_link"/>
<child link="pinza_link_B"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="pinza_link_B">
<visual>
<geometry>
<box size="0.002 0.04 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.025 0 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.025 0 0"/>
<geometry>
<box size="0.002 0.04 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.025 0 0"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_pinza_D" type="fixed">
<parent link="pinza_link"/>
<child link="pinza_link_SI"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="pinza_link_SI">
<visual>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00725 0.021 0"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
<joint name="servo_pinza_I" type="fixed">
<parent link="pinza_link"/>
<child link="pinza_link_SD"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="1 0 0" />
</joint>
<link name="pinza_link_SD">
<visual>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
<geometry>
<box size="0.0375 0.002 0.032"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00725 -0.021 0"/>
<mass value="0.0005"/>
<default_inertia />
</inertial>
</link>
</macro>
</robot>
For the macros file, we will create a file named "brazo-macros.xacro" within the folder "src" of our package with the content that is shown below:
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller">
<macro name="default_inertia">
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</macro>
<macro name="default_inertia_servos">
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</macro>
<macro name="default_limit">
<limit effort="100.0" lower="-2.62" upper="2.62" velocity="3"/>
</macro>
<macro name="default_dynamics">
<dynamics fricction="0" damping="0" />
</macro>
<macro name="servo" params="nombre">
<link name="${nombre}_link">
<visual>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.0905"/>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0905"/>
<geometry>
<box size="0.04 0.032 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0905"/>
<mass value="0.055"/>
<default_inertia_servos />
</inertial>
</link>
</macro>
<macro name="base" params="nombre">
<link name="${nombre}_link_B">
<visual>
<geometry>
<box size="0.04 0.032 0.020"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.0555"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0555"/>
<geometry>
<box size="0.04 0.032 0.02"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0555"/>
<mass value="0.001"/>
<default_inertia />
</inertial>
</link>
</macro>
<macro name="soporte" params="nombre simetrico lado">
<link name="${nombre}_link_S${lado}">
<visual>
<geometry>
<box size="0.002 0.032 0.067"/>
</geometry>
<origin rpy="0 0 0" xyz="${simetrico*0.021} 0 0.022"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="${simetrico*0.021} 0 0.022"/>
<geometry>
<box size="0.002 0.032 0.067"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${simetrico*0.021} 0 0.022"/>
<mass value="0.001"/>
<default_inertia />
</inertial>
</link>
</macro>
</robot>
For the placement file, we will create a file named "brazo.urdf.xacro" within the folder "src" of our package with the content that is shown below:
<?xml version="1.0"?>
<robot name="MYRAbot-arm"
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">
<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
<link name="world" />
<brazo parent="world">
<origin rpy="0 0 0" xyz="0 0 0"/>
</brazo>
</robot>
Model analysis
As we have used xacro in order to simplify and to make the code more editable, we will execute the next commands in a terminal in order to obtain the URDF file processed:
roscd brazo_fer_modelo
cd urdf
rosrun xacro xacro.py brazo.urdf.xacro > brazo.urdfWe will execute the next command in a terminal to check the model:
rosrun urdf_parser check_urdf brazo.urdfWe must obtain something similar to the following:
robot name is: MYRAbot_arm
---------- Successfully Parsed XML ---------------
root Link: world has 1 child(ren)
child(1): base_link
child(1): hombro_link
child(1): brazo_link
child(1): antebrazo_link
child(1): muneca_link
child(1): pinza_link
child(1): pinza_link_B
child(2): pinza_link_SI
child(3): pinza_link_SD
child(2): muneca_link_B
child(3): muneca_link_SI
child(4): muneca_link_SD
child(5): muneca_link_P
child(6): muneca_link_PS
child(2): antebrazo_link_B
child(3): antebrazo_link_SI
child(4): antebrazo_link_SD
child(2): brazo_link_B
child(3): brazo_link_SI
child(4): brazo_link_SDWe will execute the next command in a terminal in order to see the graphic tree generated in a .pdf file:
rosrun urdf_parser urdf_to_graphiz brazo.urdfWe must obtain something similar to the following:
Visual test of the model
We will create a file named "brazo_rviz.launch" within the folder "launch" of our package with the content that is shown below in order to display the model and to check the joints:
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find brazo_fer_modelo)/urdf/brazo.urdf.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<param name="use_gui" value="true"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" />
</launch>
We will execute the next command in a terminal in order to start the launcher:
roslaunch brazo_fer_modelo brazo_rviz.launchThe launcher starts rviz and the GUI of joint_state_publisher. We have to set in "Global Options" the field "Fixed Frame" to "/world" link. We can change the position of the joints using the sliders of the GUI of joint_state_publisher. We must obtain something similar to the following:
Control of the joints for simulation
We have to add a controller to each mobile joint in order to simulate the servo motors Dynamixel AX-12A in gazebo
Previous steps
We will execute the next commands in a terminal in order to install the control stacks of the PR2 robot (pr2_simulator, pr2_mechanism and [http://wiki.ros.org/pr2_controllers pr2_controllers):
sudo atp-get update
sudo apt-get install ros-VERSIÓN_ROS-pr2-simulator ros-VERSIÓN_ROS-pr2-mechanism ros-VERSIÓN_ROS-pr2-controllersTo add controllers to mobile joints
Modification of the URDF
We have to set the materials, properties, transmissions and controllers for our URDF model in gazebo.
We will add the next code lines before the tag "</robot>" within the file "brazo-macros.xacro":
<macro name="gazebo_propiedades_link" params="nombre material">
<gazebo reference="${nombre}">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<material>Gazebo/${material}</material>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</macro>
<macro name="gazebo_propiedades_joint" params="nombre">
<gazebo reference="${nombre}">
<erp>0.1</erp>
<stopKd value="1000000.0" />
<stopKp value="10000000.0" />
<fudgeFactor value="0.5" />
</gazebo>
</macro>
<gazebo>
<controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
<robotNamespace>brazo</robotNamespace>
</controller:gazebo_ros_controller_manager>
</gazebo>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="base_trans">
<actuator name="base_motor" />
<joint name="base" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="arti1_trans">
<actuator name="arti1_motor" />
<joint name="arti1" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="arti2_trans">
<actuator name="arti2_motor" />
<joint name="arti2" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="arti3_trans">
<actuator name="arti3_motor" />
<joint name="arti3" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
<transmission type="pr2_mechanism_model/SimpleTransmission" name="pinza_trans">
<actuator name="pinza_motor" />
<joint name="pinza" />
<mechanicalReduction>1.0</mechanicalReduction>
<motorTorqueConstant>1.0</motorTorqueConstant>
</transmission>
We will add the next code lines before the tag "</macro>" within the file "brazo.xacro":
<gazebo_propiedades_link nombre="base_brazo_link" material="Black" />
<gazebo_propiedades_link nombre="hombro_link" material="Black" />
<gazebo_propiedades_link nombre="brazo_link" material="Black" />
<gazebo_propiedades_link nombre="antebrazo_link" material="Black" />
<gazebo_propiedades_link nombre="muneca_link" material="Black" />
<gazebo_propiedades_link nombre="brazo_link_B" material="White" />
<gazebo_propiedades_link nombre="brazo_link_SI" material="White" />
<gazebo_propiedades_link nombre="brazo_link_SD" material="White" />
<gazebo_propiedades_link nombre="antebrazo_link_B" material="White" />
<gazebo_propiedades_link nombre="antebrazo_link_SI" material="White" />
<gazebo_propiedades_link nombre="antebrazo_link_SD" material="White" />
<gazebo_propiedades_link nombre="muneca_link_B" material="White" />
<gazebo_propiedades_link nombre="muneca_link_SI" material="White" />
<gazebo_propiedades_link nombre="muneca_link_SD" material="White" />
<gazebo_propiedades_link nombre="muneca_link_P" material="White" />
<gazebo_propiedades_link nombre="muneca_link_PS" material="White" />
<gazebo_propiedades_link nombre="pinza_link" material="White" />
<gazebo_propiedades_link nombre="pinza_link_B" material="White" />
<gazebo_propiedades_link nombre="pinza_link_SI" material="White" />
<gazebo_propiedades_link nombre="pinza_link_SD" material="White" />
<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" />
Creation of the configuration file
We have used two controllers for each mobile joint, one for velocity control and one for position control. Each controller is a PID regulator. We will create a file named "controllers.yaml" within our package with the content that is shown below in order to configure the PID regulators of each mobile joint:
base_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: base
pid: &base_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
arti1_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: arti1
pid: &arti1_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
arti2_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: arti2
pid: &arti2_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
arti3_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: arti3
pid: &arti3_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
pinza_pos_controller:
type: robot_mechanism_controllers/JointPositionController
joint: pinza
pid: &pinza_pos
p: 2.0
i: 0.5
d: 0.0
i_clamp: 10.0
base_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: base
pid: &base_vel
p: 2.0
i: 0.5
i_clamp: 10.0
arti1_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: arti1
pid: &arti1_vel
p: 2.0
i: 0.5
i_clamp: 10.0
arti2_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: arti2
pid: &arti2_vel
p: 2.0
i: 0.5
i_clamp: 10.0
arti3_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: arti3
pid: &arti3_vel
p: 2.0
i: 0.5
i_clamp: 10.0
pinza_vel_controller:
type: robot_mechanism_controllers/JointVelocityController
joint: pinza
pid: &pinza_vel
p: 2.0
i: 0.5
i_clamp: 10.0Loading the model in gazebo
We will create a file named "brazo_gazebo.launch" within the folder "launch" of our package with the content that is shown below in order to start gazebo and to load the model and the controllers:
<launch>
<param name="/use_sim_time" value="true" />
<node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
<node name="gazebo_gui" pkg="gazebo" type="gui" />
<group ns="brazo">
<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_arm_model)/urdf/brazo.urdf.xacro'" />
<node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model brazo" respawn="false" output="screen" />
<rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>
<node name="spawn_controller_brazo" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
</group>
</launch>
We will execute the next command in a terminal in order to start the launcher:
roslaunch brazo_fer_modelo brazo.gazebo.launchWe can see the arm model in vertical position placed in the point xyz=(0 0 0) of a empty world in gazebo.
MYRAbot's arm model in gazebo
We will execute the next command in a new terminal in order to check that the controllers have been loaded properly:
rostopic echoWe must obtain something similar to the following, where the topic NAME_CONTROLLER/command is the set point and the topic NAME_CONTROLLER/state is the status data:
/brazo/arti1_pos_controller/command
/brazo/arti1_pos_controller/state
/brazo/arti1_vel_controller/command
/brazo/arti1_vel_controller/state
/brazo/arti2_pos_controller/command
/brazo/arti2_pos_controller/state
/brazo/arti2_vel_controller/command
/brazo/arti2_vel_controller/state
/brazo/arti3_pos_controller/command
/brazo/arti3_pos_controller/state
/brazo/arti3_vel_controller/command
/brazo/arti3_vel_controller/state
/brazo/base_pos_controller/command
/brazo/base_pos_controller/state
/brazo/base_vel_controller/command
/brazo/base_vel_controller/state
/brazo/joint_states
/brazo/mechanism_statistics
/brazo/pinza_pos_controller/command
/brazo/pinza_pos_controller/state
/brazo/pinza_vel_controller/command
/brazo/pinza_vel_controller/state
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/rosout
/rosout_agg
/tfInterface program (gazebo-control programs)
We have created a program that communicates the arm's control programs with gazebo controllers. We will create a file named "control_modelo.cpp" within the folder "src" of our package with the content that is shown below:
#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
#include "math.h"
#define PI 3.14159265
void mover(const brazo_fer::WriteServos& move)
{
ros::NodeHandle n;
ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("brazo/base_pos_controller/command", 1);
ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_pos_controller/command", 1);
ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_pos_controller/command", 1);
ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_pos_controller/command", 1);
ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("brazo/base_vel_controller/command", 1);
ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_vel_controller/command", 1);
ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_vel_controller/command", 1);
ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_vel_controller/command", 1);
brazo_fer::Servos position = move.posicion;
brazo_fer::Servos speed = move.velocidad;
brazo_fer::Servos torque = move.par;
std_msgs::Float64 base_pos_command, arti1_pos_command, arti2_pos_command, arti3_pos_command;
std_msgs::Float64 base_vel_command, arti1_vel_command, arti2_vel_command, arti3_vel_command;
if (torque.base != 0 && (position.base >= 0 && position.base <= 1023) && (speed.base >= 0 && speed.base <= 1023))
{
base_pos_command.data = ((((position.base * 300)/1023)*PI)/180)-2.62;
base_vel_command.data = speed.base/511;
}
if (torque.arti1 != 0 && (position.arti1 >= 0 && position.arti1 <= 1023) && (speed.arti1 >= 0 && speed.arti1 <= 1023))
{
arti1_pos_command.data = ((((position.arti1*300)/1023)*PI)/180)-2.62;
arti1_vel_command.data = speed.arti1/511;
}
if (torque.arti2 != 0 && (position.arti2 >= 0 && position.arti2 <= 1023) && (speed.arti2 >= 0 && speed.arti2 <= 1023))
{
arti2_pos_command.data = ((((position.arti2*300)/1023)*PI)/180)-2.62;
arti2_vel_command.data = speed.arti2/511;
}
if (torque.arti3 != 0 && (position.arti3 >= 0 && position.arti3 <= 1023) && (speed.arti3 >= 0 && speed.arti3 <= 1023))
{
arti3_pos_command.data = ((((position.arti3*300)/1023)*PI)/180)-2.62;
arti3_vel_command.data = speed.arti3/511;
}
base_pos_pub_.publish(base_pos_command);
arti1_pos_pub_.publish(arti1_pos_command);
arti2_pos_pub_.publish(arti2_pos_command);
arti3_pos_pub_.publish(arti3_pos_command);
base_vel_pub_.publish(base_vel_command);
arti1_vel_pub_.publish(arti1_vel_command);
arti2_vel_pub_.publish(arti2_vel_command);
arti3_vel_pub_.publish(arti3_vel_command);
}
void pinza(const brazo_fer::WriteServos& pinza)
{
ros::NodeHandle n;
ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_pos_controller/command", 1);
ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_vel_controller/command", 1);
brazo_fer::Servos position = pinza.posicion;
brazo_fer::Servos speed = pinza.velocidad;
brazo_fer::Servos torque = pinza.par;
std_msgs::Float64 pinza_pos_command;
std_msgs::Float64 pinza_vel_command;
if (torque.pinza != 0 && (position.pinza >= 0 && position.pinza <= 1023) && (speed.pinza >= 0 && speed.pinza <= 1023))
{
pinza_pos_command.data = ((((position.pinza*300)/1023)*PI)/180)-2.62;
pinza_vel_command.data = speed.pinza/511;
}
pinza_pos_pub_.publish(pinza_pos_command);
pinza_vel_pub_.publish(pinza_vel_command);
}
void pe(const sensor_msgs::JointState& pe)
{
ros::NodeHandle n;
ros::Publisher pose_pub_=n.advertise<brazo_fer::ReadServos>("pose_arm", 1);
brazo_fer::ReadServos estado;
estado.posicion.base = ((((pe.position[0]+2.62)*180)/PI)*1023)/300;
estado.posicion.arti1 = ((((pe.position[1]+2.62)*180)/PI)*1023)/300;
estado.posicion.arti2 = ((((pe.position[2]+2.62)*180)/PI)*1023)/300;
estado.posicion.arti3 = ((((pe.position[3]+2.62)*180)/PI)*1023)/300;
estado.posicion.pinza = ((((pe.position[4]+2.62)*180)/PI)*1023)/300;
estado.corriente.base = pe.effort[0]*1000;
estado.corriente.arti1 = pe.effort[1]*1000;
estado.corriente.arti2 = pe.effort[2]*1000;
estado.corriente.arti3 = pe.effort[3]*1000;
estado.corriente.pinza = pe.effort[4]*1000;
pose_pub_.publish(estado);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "modelo_brazo");
ros::NodeHandle n;
ros::Subscriber move_sub_= n.subscribe("move_arm", 1, mover);
ros::Subscriber hand_sub_= n.subscribe("hand_arm", 1, pinza);
ros::Subscriber pe_sub_= n.subscribe("brazo/joint_states", 1, pe);
ros::Publisher base_pos_pub_=n.advertise<std_msgs::Float64>("brazo/base_pos_controller/command", 1);
ros::Publisher arti1_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_pos_controller/command", 1);
ros::Publisher arti2_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_pos_controller/command", 1);
ros::Publisher arti3_pos_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_pos_controller/command", 1);
ros::Publisher pinza_pos_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_pos_controller/command", 1);
ros::Publisher base_vel_pub_=n.advertise<std_msgs::Float64>("brazo/base_vel_controller/command", 1);
ros::Publisher arti1_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti1_vel_controller/command", 1);
ros::Publisher arti2_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti2_vel_controller/command", 1);
ros::Publisher arti3_vel_pub_=n.advertise<std_msgs::Float64>("brazo/arti3_vel_controller/command", 1);
ros::Publisher pinza_vel_pub_=n.advertise<std_msgs::Float64>("brazo/pinza_vel_controller/command", 1);
ros::Publisher pose_pub_=n.advertise<brazo_fer::ReadServos>("pose_arm", 1);
ros::spin();
return 0;
}
We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:
rosbuild_add_executable(control_modelo src/control_modelo.cpp)
We will execute the next commands in a terminal in order to compile and create the executable file:
roscd brazo_fer_modelo
makeSimulation of control programs
First we have to add the next code line before the tag "</launch>" within the file "brazo_gazebo.launch" in order to launch the previous interface program:
<node name="control_modelo" pkg="brazo_fer_modelo" type="control_modelo" />
First control program (Come here)
We use the same control program that we had created for the real arm. First we will execute the next command in a terminal in order to launch the simulator with the arm model:
roslaunch brazo_fer_modelo brazo_gazebo.launchWhen the simulator gazebo is started, we will execute the next command in a new terminal in order to start the control program:
rosrun brazo_fer control_v01We will execute the next command in a new terminal in order to publish the goal point in the topic "point":
rostopic pub point geometry_msgs/Point '{x: 100, y: 0, z: 180}' --onceThe arm will move to the goal point. We can see the arm moving from the start point to the goal point in the next video:
Second control program (Grasp the tin)
First we will create a file named "lata.urdf" within the folder "urdf" of our package with the content that is shown below in order to create the tin model:
<?xml version="1.0"?>
<robot name="lata">
<link name="world" />
<joint name="fixed" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.03" length="0.125" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.0625" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0625" />
<geometry>
<cylinder radius="0.03" length="0.125" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.0625" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="lata_link">
<material>Gazebo/Red</material>
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</robot>
Also we have to add the next code line to the file "brazo_gazebo.launch" in order to launch the tin model in gazebo in the position xyz = (-0.15 0.21 0).
<node name="spawn_lata" pkg="gazebo" type="spawn_model" args="-file $(find brazo_fer_modelo)/lata.urdf -urdf -x -0.15 -y 0.21 -model lata" respawn="false" output="screen" />
We use the same control program that we had created for the real arm. First we will execute the next command in a terminal in order to launch the simulator with the arm model:
roslaunch brazo_fer_modelo brazo_gazebo.launchWhen the simulator gazebo is started, we will execute the next command in a new terminal in order to start the control program:
rosrun brazo_fer control_v02We will execute the next command in a new terminal in order to publish the placement point of the tin in the topic "pick_point":
rostopic pub point geometry_msgs/Point '{x: 145, y: 0, z: 180}' --onceThe arm will move to the initial position. When we publish the placement point of the tin, the arm will move to grasp the tin. We can see the arm moving to initial position and grasping the tin in the next video:
