Note: This tutorial assumes that you have completed the previous tutorials: Writing a realtime joint controller.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Simple URDF-Controller Example

Description: This tutorial shows you how to build a simple model with URDF, and how to link it with the controller. In addition, useful tips on building and verifying of the created robot model are given.

Keywords: URDF,controller,gazebo

Tutorial Level: ADVANCED

The following code is a simple differential drive robot.

   1 <?xml version="1.0"?>
   2 <robot name="robot"
   3 
   4        xmlns:xi="http://www.w3.org/2001/XInclude"
   5        xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
   6        xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
   7        xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
   8        xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
   9        xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
  10        xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
  11        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
  12        xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
  13        xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
  14        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
  15        xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
  16 
  17 
  18         <!-- Links: inertial,visual,collision -->
  19         <link name="base_link">
  20                 <inertial>
  21                         <!-- origin is relative -->
  22                         <origin xyz="0 0 0" rpy="0 0 0"/>
  23                         <mass value="5"/>
  24                         <inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
  25                 </inertial>
  26                 <visual>
  27                         <geometry>
  28                                 <box size="0.5 0.1 0.1"/>
  29                         </geometry>
  30                 </visual>
  31                 <collision>
  32                          <!-- origin is relative -->
  33                         <origin xyz="0 0 0"/>
  34                         <geometry>
  35                                 <box size="0.5 0.1 0.1"/>
  36                         </geometry>
  37                 </collision>
  38         </link>
  39 
  40         <link name="wheel1">
  41                 <inertial>
  42                         <origin xyz="0 0 0" rpy="0 0 0"/>
  43                         <mass value="1"/>
  44                         <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
  45                 </inertial>
  46                 <visual>
  47                         <origin xyz="0 0 0" rpy="0 1.57 0"/>
  48                         <geometry>
  49                                 <cylinder radius="0.110" length="0.086"/>
  50                         </geometry>
  51                 </visual>
  52                 <collision>
  53                         <origin xyz="0 0 0" rpy="0 1.57 0"/>
  54                         <geometry>
  55                                 <cylinder radius="0.110" length="0.086"/>
  56                         </geometry>
  57                 </collision>
  58         </link>
  59 
  60         <link name="wheel2">
  61                 <inertial>
  62                         <origin xyz="0 0 0" rpy="0 0 0"/>
  63                         <mass value="1"/>
  64                         <inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
  65                 </inertial>
  66                 <visual>
  67                         <origin xyz="0 0 0" rpy="0 1.57 0"/>
  68                         <geometry>
  69                                 <cylinder radius="0.110" length="0.086"/>
  70                         </geometry>
  71                 </visual>
  72                 <collision>
  73                         <origin xyz="0 0 0" rpy="0 1.57 0"/>
  74                         <geometry>
  75                                 <cylinder radius="0.110" length="0.086"/>
  76                         </geometry>
  77                 </collision>
  78         </link>
  79 
  80 
  81         <!-- Joints: axis,limit,dynamics,orign,parent,child -->
  82         <joint name="joint_w1" type="continuous">
  83                 <parent link="base_link"/>
  84                 <child link="wheel1"/>
  85                 <origin xyz="0.35 0 0"/>
  86                 <axis xyz="0 1 0"/>
  87         </joint>
  88 
  89 
  90         <joint name="joint_w2" type="continuous">
  91                 <parent link="base_link"/>
  92                 <child link="wheel2"/>
  93                 <origin xyz="-0.35 0 0"/>
  94                 <axis xyz="0 1 0"/>
  95         </joint>
  96 
  97         <!-- Transmission is important to link the joints and the controller -->
  98         <transmission name="joint_w1_trans" type="SimpleTransmission">
  99                 <actuator name="joint_w1_motor" />
 100                 <joint name="joint_w1" />
 101                 <mechanicalReduction>1</mechanicalReduction>
 102                 <motorTorqueConstant>1</motorTorqueConstant>
 103         </transmission>
 104 
 105         <transmission name="joint_w2_trans" type="SimpleTransmission">
 106                 <actuator name="joint_w2_motor" />
 107                 <joint name="joint_w2" />
 108                 <mechanicalReduction>1</mechanicalReduction>
 109                 <motorTorqueConstant>1</motorTorqueConstant>
 110         </transmission>
 111 
 112         <!-- Colour -->
 113         <gazebo reference="base_link">
 114           <material>Gazebo/Green</material>
 115         </gazebo>
 116 
 117         <gazebo reference="wheel1">
 118           <material>Gazebo/Red</material>
 119         </gazebo>
 120 
 121         <gazebo reference="wheel2">
 122           <material>Gazebo/Blue</material>
 123         </gazebo>
 124 
 125 
 126  <gazebo>
 127       <controller:gazebo_ros_time name="gazebo_ros_time" plugin="libgazebo_ros_time.so">
 128         <alwaysOn>true</alwaysOn>
 129         <updateRate>1000.0</updateRate>
 130         <interface:audio name="dummy_gazebo_ros_time_iface_should_not_be_here"/>
 131       </controller:gazebo_ros_time>
 132 
 133       <!-- PR2_ACTARRAY -->
 134       <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
 135         <alwaysOn>true</alwaysOn>
 136         <updateRate>1000.0</updateRate>
 137         <interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
 138       </controller:gazebo_ros_controller_manager>
 139 
 140   </gazebo>
 141 
 142 </robot>

To take a look at the robot you can use this launch file:

   1 <launch>
   2   <!-- start world -->
   3   <include file="$(find gazebo_worlds)/launch/office_world.launch"/>
   4 
   5   <!-- start controller manager (rviz) -->
   6   <include file="$(find pr2_controller_manager)/controller_manager.launch"/>
   7 
   8   <!-- load robot -->
   9   <param name="robot_description" textfile="$(find tutorial_controller)/robot.xml" />
  10 
  11 
  12   <!-- push robot_description to factory and spawn robot in gazebo -->
  13   <node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model pr2 -z 0.5" respawn="false" output="screen" />
  14 
  15 </launch>



It's always good to look at your model in different views!



In the gazebo window click "View" and then try Show Physics,Show Bounding Boxes, Show Joints. If everything looks fine your done with your model.

The transmission is important to have access to the joints over your controller. So you can link them into your controller init() method:

   1 ...
   2 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
   3                             ros::NodeHandle &n)
   4 {
   5   std::string wheel1, wheel2;
   6 
   7   ///Link all Joints
   8   ////////////////////////////////////////////////////////////////////////
   9   if (!n.getParam("wheel1", wheel1))
  10   {
  11     ROS_ERROR("No joint given in namespace: '%s')",
  12               n.getNamespace().c_str());
  13     return false;
  14   }
  15 
  16   wheel1_state = robot->getJointState(wheel1);
  17   if (! wheel1_state )
  18   {
  19     ROS_ERROR("MyController could not find joint named '%s'",
  20                wheel1.c_str());
  21     return false;
  22   }
  23 
  24    ////////////////////////////////////////////////////////////////////////
  25   if (!n.getParam("wheel2", wheel2))
  26   {
  27     ROS_ERROR("No joint given in namespace: '%s')",
  28               n.getNamespace().c_str());
  29     return false;
  30   }
  31 
  32   wheel2_state = robot->getJointState(wheel2);
  33   if (!wheel2_state )
  34   {
  35     ROS_ERROR("MyController could not find joint named '%s'",
  36                wheel2.c_str());
  37     return false;
  38   }
  39 ...

The yaml-file look like this:

   1   my_controller_ns:
   2     type: MyControllerPlugin
   3     wheel1: joint_w1
   4     wheel2: joint_w2
   5 
   6     pid_steer:
   7       p: 150.0
   8       i: 2.0
   9       d: 30.0
  10       i_clamp: 1.0

Wiki: pr2_mechanism/Tutorials/SImple URDF-Controller Example (last edited 2011-04-01 07:58:35 by hsu)