Robotics StackExchange | Archived questions

Differential drive robot on ROS Indigo slipping

Dear all,

For the last couple of days, I have been trying to create a URDF model of a simple (or that is what I thought) differential drive robot (libgazebo_ros_diff_drive and libgazebo_ros_joint_state_publisher plugins) with a laser sensor (libgazebo_ros_laser) mounted on it and simulate it using gazebo (using ROS indigo).

The laser readings are just fine, the problem comes when sending twist commands to the robot: it doesn't move. Activating the option View->Wireframe in gazebo's gui makes it easy to see that the wheels are actually spinning, but the robot does not move. It feels like it is stuck to the ground or slipping on ice.

I am sure that I am missing something, but I can't figure out what. After days of looking at other examples that work, I am totally lost. Any help would be greatly appreciated.

Here is the URDF file:

<?xml version="1.0"?>
<robot name="cleaning_robot">
<!--
<link name="base_footprint"/>
-->
<link name="base_link">
  <visual>
    <geometry>
      <cylinder length="0.25" radius="0.26"/>
    </geometry>
    <material name="gray">
      <color rgba="0.8 0.8 0.8 0.7"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.3" radius="0.32"/>
    </geometry>
  </collision>
  <inertial>
      <origin xyz="0.01 0 0" rpy="0 0 0"/>
      <mass value="10.0"/>
      <inertia
        ixx="0.074" ixy="0.0" ixz="0.0"
        iyy="0.12" iyz="0.0"
        izz="0.15"/>
    </inertial>
</link>

<gazebo reference="base_link">
  <material value="Gazebo/White"/>
</gazebo>

<link name="left_wheel_link">
  <visual>
    <geometry>
      <cylinder length="0.04" radius="0.145"/>
    </geometry>
    <origin xyz="0 0 0" rpy="1.57075 0 0"/>
    <material name="blue">
      <color rgba="0 0 .8 1"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.04" radius="0.145"/>
    </geometry>
    <origin xyz="0 0 0" rpy="1.57075 0 0"/>
  </collision>
  <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.01"/>
      <inertia
        ixx="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0"
        izz="0.01"/>
    </inertial>
</link>

<gazebo reference="left_wheel_link">
  <mu1>1.0</mu1>
  <mu2>1.0</mu2>
  <kp>1000000.0</kp> 
  <kd>100.0</kd>
  <minDepth>0.001</minDepth>
  <maxVel>1.0</maxVel>
</gazebo>

<joint name="base_to_left_wheel" type="continuous">
  <parent link="base_link"/>
  <child link="left_wheel_link"/>
  <origin xyz="0 0.3 0" rpy="0 0 0"/>
  <axis xyz="0 1 0"/>
</joint>

<link name="right_wheel_link">
  <visual>
    <geometry>
      <cylinder length="0.04" radius="0.145"/>
    </geometry>
    <origin xyz="0 0 0" rpy="1.57075 0 0"/>
    <material name="blue"/>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.04" radius="0.145"/>
    </geometry>
    <origin xyz="0 0 0" rpy="1.57075 0 0"/>
  </collision>
  <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.01"/>
      <inertia
        ixx="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0"
        izz="0.01"/>
    </inertial>
</link>
<gazebo reference="right_wheel_link">
  <mu1>1.0</mu1>
  <mu2>1.0</mu2>
  <kp>1000000.0</kp>
  <kd>100.0</kd>
  <minDepth>0.001</minDepth>
  <maxVel>1.0</maxVel>
</gazebo>


<joint name="base_to_right_wheel" type="continuous">
  <parent link="base_link"/>
  <child link="right_wheel_link"/>
  <origin xyz="0 -0.3 0" rpy="0 0 0"/>
  <axis xyz="0 1 0"/>
</joint>

<link name="front_wheel_link">
  <visual>
    <geometry>
      <sphere radius="0.009"/>
    </geometry>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <material name="blue"/>
  </visual>
  <collision>
    <geometry>
      <sphere radius="0.009"/>
    </geometry>
    <origin xyz="0 0 0" rpy="1.0 0 0"/>
  </collision>
  <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.01"/>
      <inertia
        ixx="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0"
        izz="0.01"/>
    </inertial>
</link>

<joint name="base_to_front_wheel" type="fixed">
  <parent link="base_link"/>
  <child link="front_wheel_link"/>
  <origin xyz="0.24 0 -0.135" rpy="0 0 0"/>
</joint>

<link name="rear_wheel_link">
  <visual>
    <geometry>
      <sphere radius="0.009"/>
    </geometry>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <material name="blue"/>
  </visual>
  <collision>
    <geometry>
      <sphere radius="0.001"/>
    </geometry>
    <origin xyz="0 0 0" rpy="1.0 0 0"/>
  </collision>
  <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.01"/>
      <inertia
        ixx="0.01" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0"
        izz="0.01"/>
    </inertial>
</link>

<joint name="base_to_rear_wheel" type="fixed">
  <parent link="base_link"/>
  <child link="rear_wheel_link"/>
  <origin xyz="-0.24 0 -0.135" rpy="0 0 0"/>
</joint>


<!-- Laser Joint definition -->

<link name="laser_mount_link">
  <inertial>
    <mass value="0.001"/>
    <origin xyz="0 0 0"/>
    <inertia ixx="0.000001" ixy="0.0" ixz="0.0" iyy="0.000001" iyz="0.0" izz="0.000001"/>
  </inertial>
</link>


<joint name="base_to_laser_mount" type="fixed">
  <parent link="base_link"/>
  <child link="laser_mount_link"/>
  <origin xyz="0.34 0 -0.022" rpy="3.14156 0 0"/>
</joint>

<link name="laser">
  <visual>
    <geometry>
      <mesh filename="package://sick_tim3xx/meshes/sick_tim3xx.stl"/>
    </geometry>
    <material name="black">
      <color rgba="1 1 1 1"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <mesh filename="package://sick_tim3xx/meshes/sick_tim3xx.stl"/>
    </geometry>
  </collision>
  <inertial>
    <mass value="0.15"/>
    <origin xyz="0 0 -0.026"/>
    <inertia ixx="0.00012499995" ixy="0.0" ixz="0.0" iyy="0.00012499995" iyz="0.0" izz="8.9999964e-05"/>
  </inertial>
</link>


<joint name="laser_mount_to_laser" type="fixed">
  <parent link="laser_mount_link"/>
  <child link="laser"/>
  <origin rpy="0 0 0" xyz="0 0 0.05609"/>
</joint>

<!-- Add the differential_drive_controller -->
<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <rosDebugLevel>Debug</rosDebugLevel>
      <publishWheelTF>false</publishWheelTF>
      <publishWheelJointState>true</publishWheelJointState>
      <alwaysOn>true</alwaysOn>
      <leftJoint>base_to_left_wheel</leftJoint>
      <rightJoint>base_to_right_wheel</rightJoint>
      <wheelSeparation>0.569</wheelSeparation>
      <wheelDiameter>0.145</wheelDiameter>
      <wheelTorque>20</wheelTorque>
      <wheelAcceleration>1.8</wheelAcceleration>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <odometrySource>world</odometrySource>
      <robotBaseFrame>base_link</robotBaseFrame>
      <updateRate>10.0</updateRate>
  </plugin>
</gazebo>

<gazebo>
    <plugin filename="libgazebo_ros_joint_state_publisher.so" name="joint_state_publisher">
      <jointName>base_to_left_wheel, base_to_right_wheel</jointName>
      <updateRate>10.0</updateRate>
      <alwaysOn>true</alwaysOn>
    </plugin>
</gazebo>


<!-- Add a simulated laser -->
<gazebo reference="laser">
  <material value="Gazebo/Black"/>
  <selfCollide>false</selfCollide>
  <sensor name="laser" type="ray">
  <always_on>true</always_on>
  <update_rate>15.0</update_rate>
  <pose>0 0 0 0 0 0</pose>
  <visualize>false</visualize>
  <ray>
    <scan>
      <horizontal>
        <samples>271</samples>
        <resolution>1</resolution>
        <min_angle>-2.357</min_angle>
        <max_angle>2.357</max_angle>
      </horizontal>
    </scan>
    <range>
      <min>0.05</min>
      <max>4.0</max>
      <resolution>0.01</resolution>
    </range>
  </ray>
  <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_laser_controller">
    <gaussianNoise>0.005</gaussianNoise>
    <alwaysOn>true</alwaysOn>
    <updateRate>15.0</updateRate>
    <topicName>scan</topicName>
    <frameName>laser</frameName>
  </plugin>
  </sensor>
</gazebo>

</robot>

Asked by Martin Peris on 2014-08-27 21:57:31 UTC

Comments

Can we have a look at the TF tree to see if anything is wrong there? You can check it by running "rosrun tf view_frames" while everything is running. This will create a PDF you can view by "evince frames.pdf". Also, you can try playing with the friction properties. I don't know much about them but you could try 100 for mu1 and mu2 instead of 1.0. Also I usually see kd set to 1.0, but again, I don't know much about what they mean.

Asked by Airuno2L on 2014-08-28 07:57:40 UTC

Answers

The wheel friction values (mu, mu2) are a bit low. Try values around 100000.

Asked by nkoenig on 2014-08-28 11:33:44 UTC

Comments

Thank you all for your answers and comments... as I suspected, it was my own fault! Take a look at this

<link name="base_link">
  <visual>
    <geometry>
      <cylinder length="0.25" radius="0.26"/> <!-- Correct visual geometry -->
    </geometry>
    <material name="gray">
      <color rgba="0.8 0.8 0.8 0.7"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <cylinder length="0.3" radius="0.32"/>  <!-- What did I smoke while doing this??
  The correct geometry would be: <cylinder length="0.25" radius="0.26"/>
  --> 
    </geometry>
  </collision>
  <inertial>
      <origin xyz="0.01 0 0" rpy="0 0 0"/>
      <mass value="10.0"/>
      <inertia
        ixx="0.074" ixy="0.0" ixz="0.0"
        iyy="0.12" iyz="0.0"
        izz="0.15"/>
    </inertial>
</link>

The visual and collision geometries are different, in this case the collision geometry was causing the wheels to "float" 0.5cm above the ground, therefore there was no "contact" with the ground plane and no movement.

Thank you again.

Asked by Martin Peris on 2014-08-28 20:18:49 UTC

Comments