Home | Tutorials | Wiki | Issues
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Differential driven robot warping when driving forward and stuttering when driving backwards

Hi everybody,

I'm modelling a differential driven robot and have two problems.

image description

image description

My first problem: The robot turns a bit right or left when I want to drive forward.

Made this short Video for explanation.

I tried to fix it by setting the wheels stiffness, damping, friction, maxVel and minDepth to different values but didn't get it working correctly.

This are my xxx.urdf.xacro files:

body.urdf.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- The body macro that is also used to generate the tail -->
  <xacro:macro name="body" params="name parent length width height mass off_x off_y off_z role pitch yaw type full_joint material Gmaterial mu1 mu2 kp kd">

    <!-- if -->
    <xacro:if value="${full_joint}">
      <joint name="${parent}_to_${name}" type="${type}">
        <parent link="${parent}"/>
        <child link="${name}_body_link"/>
        <origin rpy= "${role} ${pitch} ${yaw}" xyz="${off_x} ${off_y} ${off_z}"/>
      </joint>
    </xacro:if>

    <!-- else -->
    <xacro:unless value="${full_joint}">
      <joint name="${parent}_to_${name}" type="${type}">
        <parent link="${parent}"/>
        <child link="${name}_body_link"/>
      </joint>
    </xacro:unless>

    <link name="${name}_body_link">
      <visual>
        <origin xyz="0 0 0"/>
        <geometry>
           <box size="${length} ${width} ${height}"/>
        </geometry>
        <material name="${material}"/>
       </visual>

       <collision>
         <origin xyz="0 0 0"/>
         <geometry>
           <box size="${length} ${width} ${height}"/>
         </geometry>
       </collision>

       <xacro:solid_cuboid_inertial
        width="${length}" depth="${width}"
        height="${height}" mass="${mass}">
        <origin xyz="${off_x} ${off_y} ${off_z}"/>
      </xacro:solid_cuboid_inertial>
    </link>

    <gazebo reference="${name}_body_link">
      <!-- <material>Gazebo/Black</material> -->
      <material>Gazebo/${Gmaterial}</material>
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
      <kp>${kp}</kp>
      <kd>${kd}</kd>
    </gazebo>
    <!--
   <gazebo reference="{parent}_to_${name}" >
   </gazebo>
   -->
  </xacro:macro>

  <!-- The sphere macro that is used to simulate the finn -->
  <xacro:macro name="finn" params="name parent reflect radius mass off_x off_y off_z role pitch yaw type material Gmaterial mu1 mu2 kp kd maxVel minDepth fdir1">

    <joint name="${parent}_to_${name}" type="${type}">
      <parent link="${parent}"/>
      <child link="${name}_sphere_link"/>
      <origin rpy= "${role} ${pitch} ${yaw}" xyz="${off_x} ${off_y} ${reflect * off_z}"/>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${name}_sphere_link">
      <visual>
        <origin xyz="0 0 0"/>
        <geometry>
           <sphere radius="${radius}"/>
        </geometry>
        <material name="${material}"/>
       </visual>

       <collision>
         <origin xyz="0 0 0"/>
         <geometry>
           <sphere radius="${radius}"/>
         </geometry>
       </collision>

       <xacro:sphere_inertial
        radius="${radius}" mass="${mass}">
        <origin xyz="${off_x} ${off_y} ${reflect * off_z}"/>
      </xacro:sphere_inertial>
    </link>

    <gazebo reference="${name}_sphere_link">
      <!-- <material>Gazebo/Black</material> -->
      <material>Gazebo/${Gmaterial}</material>
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
      <kp>${kp}</kp>
      <kd>${kd}</kd>
      <maxVel>${maxVel}</maxVel>
      <minDepth>${minDepth}</minDepth>
      <fdir1>${fdir1}</fdir1>
    </gazebo>

    <gazebo reference="{parent}_to_${name}" >
      <kp>${kp}</kp>
      <kd>${kd}</kd>
    </gazebo>

  </xacro:macro>


  <!-- Definitions of all single parts -->
  <xacro:body
       name="main"
       parent="base_link"
       length="0.14"
       width="0.22"
       height="0.09"
       mass="1.709"
       off_x="0"
       off_y="0"
       off_z="0"
        role="0"
        pitch="0"
        yaw="0"
       type="fixed"
       full_joint="false"
       material="body_mat"
       Gmaterial="Black"
        mu1="0.5"
        mu2="50.0"
        kp="100000000.0"
        kd="1.0"
 />

  <!-- off_x = half body + half tail-->
  <xacro:body
       name="tail"
       parent="main_body_link"
       length="0.22"
       width="0.10"
       height="0.02"
       mass="0.242"
       off_x="${-0.11-0.07}"
       off_y="0"
       off_z="0"
       role="0"
       pitch="0"
       yaw="0"
       type="fixed"
       full_joint="true"
       material="tail_mat"
       Gmaterial="Black"
       mu1="0.5"
       mu2="50.0"
       kp="100000000.0"
       kd="1.0"

 />

  <xacro:finn
       name="upper_finn"
       parent="main_body_link"
        reflect="1"
       radius="0.01"
       mass="0.050"
       off_x="${-0.07-0.165}"
       off_y="0"
       off_z="${0.055-0.01}"
       role="${PI/2}"
       pitch="0"
       yaw="0"
       type="fixed"
       material="u_sphere_mat"
       Gmaterial="Orange"
       mu1="0"
       mu2="0"
       kp="10000000"
       kd="1"
       maxVel="1000"
       minDepth="0"
       fdir1="0 0 1"
 />

  <xacro:finn
       name="lower_finn"
       parent="main_body_link"
       reflect="-1"
       radius="0.01"
       mass="0.050"
       off_x="${-0.07-0.165}"
       off_y="0"
       off_z="${0.055-0.01}"
       role="${PI/2}"
       pitch="0"
       yaw="0"
       type="fixed"
       material="l_sphere_mat"
       Gmaterial="Blue"
       mu1="0"
       mu2="0"
       kp="10000000"
       kd="1"
       maxVel="1000"
       minDepth="0"
       fdir1="0 0 1"

 />

</robot>

wheel.urdf.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- The "wheel" macro defines a wheel. -->
  <xacro:macro name="wheel" params="name parent reflect length radius mass off_x off_y off_z role pitch yaw type material Gmaterial mu1 mu2 kp kd maxVel minDepth fdir1">

    <joint name="${parent}_to_${name}_wheel_joint" type="${type}">
      <parent link="${parent}"/>
      <child link="${name}_wheel_link"/>
      <origin rpy= "${role} ${pitch} ${yaw}" xyz="${off_x} ${reflect*off_y} ${off_z}"/>
      <axis xyz="0 0 1"/>      
      <dynamics damping="0.1" friction="100.0"/>
      <!-- <dynamics damping="1.0" friction="1.0" spring_stiffness="1" /> -->
      <!-- <limit effort="100" velocity="100" /> -->
    </joint>

    <link name="${name}_wheel_link">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${material}"/>
      </visual>

      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
      </collision>

      <xacro:wheel_inertial
         rad="${radius}" height="${length}" mass="${mass}">
          <origin rpy="${PI/2} 0 0" xyz="0 0 0"/>
      </xacro:wheel_inertial>
    </link>

    <gazebo reference="${name}_wheel_link">
      <!-- <material>Gazebo/Black</material> -->
      <material>Gazebo/${Gmaterial}</material>
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
      <kp>${kp}</kp>
      <kd>${kd}</kd>
      <maxVel>${maxVel}</maxVel>
      <minDepth>${minDepth}</minDepth>
      <fdir1>${fdir1}</fdir1>
    </gazebo>

    <gazebo reference="${parent}_to_${name}_wheel_joint}" >
      <!--
     <kp>100000000.0</kp>
     <kd>1.0</kd>
     -->
      <kp>${kp}</kp>
      <kd>${kd}</kd>
    </gazebo>

    <!--
   <transmission name="${parent}_to_${name}_wheel_trans">
     <type>pr2_mechanism_model/SimpleTransmission</type>
     <joint name="${parent}_to_${name}_wheel_joint" />
     <actuator name="${name}_wheel_motor">
       <hardwareInterface>EffortJointInterface</hardwareInterface>
       <mechanicalReduction>${reflect * 624/35 * 80/19}</mechanicalReduction>
     </actuator>
   </transmission>
   -->

  </xacro:macro>

  <!-- off_y = half body + half wheel + 1cm gab between. -->
  <xacro:wheel  
       name="left"  
       reflect="1"  
       parent="main_body_link"  
       length="0.04"  
       radius="0.12"  
       mass="0.242"  
       off_x="0"  
       off_y="${0.11+0.02+0.01}"
       off_z="0"
        role="-${PI/2}"
        pitch="0"
        yaw="0"  
       type="continuous"
       material="l_wheel_mat"  
       Gmaterial="Green"
       mu1="100"
       mu2="100"
       kp="100000000"
       kd="1"
        maxVel="100000000"
        minDepth="0"
        fdir1="0 0 1"
 />

  <xacro:wheel  
       name="right"  
       reflect="-1"  
       parent="main_body_link"  
       length="0.04"  
       radius="0.12"  
       mass="0.242"  
       off_x="0"  
       off_y="${0.11+0.02+0.01}"  
       off_z="0"  
       role="-${PI/2}"
       pitch="0"
       yaw="0"
       type="continuous"  
       material="r_wheel_mat"  
       Gmaterial="Red"
       mu1="100"
       mu2="100"
       kp="100000000"
       kd="1"
        maxVel="100000000"
       minDepth="0"
       fdir1="0 0 1"
 />

  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <alwaysOn>true</alwaysOn>
      <!--updateRate>${update_rate}</updateRate-->
      <leftJoint>main_body_link_to_right_wheel_joint</leftJoint>
      <rightJoint>main_body_link_to_left_wheel_joint</rightJoint>
      <wheelSeparation>0.2800</wheelSeparation>
      <wheelDiameter>0.2400</wheelDiameter>
      <torque>20</torque>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_footprint</robotBaseFrame>
    </plugin>
  </gazebo>

</robot>

common.urdf.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Degree-to-radian conversions -->
  <xacro:property name="degrees_45" value="0.785398163"/>
  <xacro:property name="degrees_90" value="1.57079633"/>
  <xacro:property name="PI" value="3.1415"/>

  <!-- Some predefined materials/colors -->
  <material name="l_wheel_mat">
    <color rgba="0 1 0 1"/>
  </material>
  <material name="r_wheel_mat">
    <color rgba="1 0 0 1"/>
  </material>
  <material name="tail_mat">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="body_mat">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="tire_mat">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="u_sphere_mat">
    <color rgba="1 0.7 0 1"/>
  </material>
  <material name="l_sphere_mat">
    <color rgba="0 0 1 1"/>
  </material>

  <!-- Null inertial element. This is needed to make the model work with
      Gazebo. -->
  <xacro:macro name="null_inertial">
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
  </xacro:macro>

  <!-- Inertia of a solid cuboid. Width is measured along the x axis, depth
      along the y axis, and height along the z axis. -->
  <xacro:macro name="solid_cuboid_inertial"
              params="width depth height mass *origin">
    <inertial>
      <xacro:insert_block name="origin"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass * (depth * depth + height * height) / 12}"
              ixy="0" ixz="0"
              iyy="${mass * (width * width + height * height) / 12}"
              iyz="0"
              izz="${mass * (width * width + depth * depth) / 12}"/>
    </inertial>
  </xacro:macro>


  <!-- Inertia of wheel. Height is measured along the z axis,
      which is the wheels's axis. -->
  <xacro:macro name="wheel_inertial"
              params="rad height mass *origin">
    <inertial>
      <xacro:insert_block name="origin"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass * (3 * rad * rad + height * height)/12}"
              ixy="0" ixz="0"
              iyy="${mass * (3 * rad * rad + height * height)/12}"
              iyz="0"
              izz="${mass * rad * rad / 2}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="sphere_inertial" params="radius mass *origin">
    <inertial>
      <mass value="${mass}" />
      <insert_block name="origin" />
      <inertia ixx="${2.0 / 5.0 * mass * radius * radius}" ixy="0.0" ixz="0.0"
              iyy="${2.0 / 5.0 * mass * radius * radius}" iyz="0.0"
              izz="${2.0 / 5.0 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

</robot>

robot.urdf.xacro:

<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find description)/urdf/robot/wheel.urdf.xacro" />
  <xacro:include filename="$(find description)/urdf/robot/body.urdf.xacro" />
  <xacro:include filename="$(find description)/urdf/robot/common.urdf.xacro" />

  <!-- base_link must have geometry so that its axes can be displayed in
      rviz. -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="body_mat"/>
    </visual>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Grey</material>
  </gazebo>

</robot>

Could anybody help me with that?

My second problem:

When I drive backwards the rover stutter when the sphere-slider hits the ground after the tail lifted up into the air. The sphere has a friction of mu1=mu2=0 and a stiffness kp=1 mil.

I think this Video will explain it.

Would be really nice if anybody could help me with that too.

Thank you and best regards,

F4B1

Differential driven robot warping when driving forward and stuttering when driving backwards

Hi everybody,

I'm modelling a differential driven robot and have two problems.

image description

image description

My first problem: The robot turns a bit right or left when I want to drive forward.

Made this short Video for explanation.

I tried to fix it by setting the wheels stiffness, damping, friction, maxVel and minDepth to different values but didn't get it working correctly.

This are my xxx.urdf.xacro files:

body.urdf.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- The body macro that is also used to generate the tail -->
  <xacro:macro name="body" params="name parent length width height mass off_x off_y off_z role pitch yaw type full_joint material Gmaterial mu1 mu2 kp kd">

    <!-- if -->
    <xacro:if value="${full_joint}">
      <joint name="${parent}_to_${name}" type="${type}">
        <parent link="${parent}"/>
        <child link="${name}_body_link"/>
        <origin rpy= "${role} ${pitch} ${yaw}" xyz="${off_x} ${off_y} ${off_z}"/>
      </joint>
    </xacro:if>

    <!-- else -->
    <xacro:unless value="${full_joint}">
      <joint name="${parent}_to_${name}" type="${type}">
        <parent link="${parent}"/>
        <child link="${name}_body_link"/>
      </joint>
    </xacro:unless>

    <link name="${name}_body_link">
      <visual>
        <origin xyz="0 0 0"/>
        <geometry>
           <box size="${length} ${width} ${height}"/>
        </geometry>
        <material name="${material}"/>
       </visual>

       <collision>
         <origin xyz="0 0 0"/>
         <geometry>
           <box size="${length} ${width} ${height}"/>
         </geometry>
       </collision>

       <xacro:solid_cuboid_inertial
        width="${length}" depth="${width}"
        height="${height}" mass="${mass}">
        <origin xyz="${off_x} ${off_y} ${off_z}"/>
      </xacro:solid_cuboid_inertial>
    </link>

    <gazebo reference="${name}_body_link">
      <!-- <material>Gazebo/Black</material> -->
      <material>Gazebo/${Gmaterial}</material>
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
      <kp>${kp}</kp>
      <kd>${kd}</kd>
    </gazebo>
    <!--
   <gazebo reference="{parent}_to_${name}" >
   </gazebo>
   -->
  </xacro:macro>

  <!-- The sphere macro that is used to simulate the finn -->
  <xacro:macro name="finn" params="name parent reflect radius mass off_x off_y off_z role pitch yaw type material Gmaterial mu1 mu2 kp kd maxVel minDepth fdir1">

    <joint name="${parent}_to_${name}" type="${type}">
      <parent link="${parent}"/>
      <child link="${name}_sphere_link"/>
      <origin rpy= "${role} ${pitch} ${yaw}" xyz="${off_x} ${off_y} ${reflect * off_z}"/>
      <dynamics damping="0.0" friction="0.0"/>
    </joint>

    <link name="${name}_sphere_link">
      <visual>
        <origin xyz="0 0 0"/>
        <geometry>
           <sphere radius="${radius}"/>
        </geometry>
        <material name="${material}"/>
       </visual>

       <collision>
         <origin xyz="0 0 0"/>
         <geometry>
           <sphere radius="${radius}"/>
         </geometry>
       </collision>

       <xacro:sphere_inertial
        radius="${radius}" mass="${mass}">
        <origin xyz="${off_x} ${off_y} ${reflect * off_z}"/>
      </xacro:sphere_inertial>
    </link>

    <gazebo reference="${name}_sphere_link">
      <!-- <material>Gazebo/Black</material> -->
      <material>Gazebo/${Gmaterial}</material>
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
      <kp>${kp}</kp>
      <kd>${kd}</kd>
      <maxVel>${maxVel}</maxVel>
      <minDepth>${minDepth}</minDepth>
      <fdir1>${fdir1}</fdir1>
    </gazebo>

    <gazebo reference="{parent}_to_${name}" >
      <kp>${kp}</kp>
      <kd>${kd}</kd>
    </gazebo>

  </xacro:macro>


  <!-- Definitions of all single parts -->
  <xacro:body
       name="main"
       parent="base_link"
       length="0.14"
       width="0.22"
       height="0.09"
       mass="1.709"
       off_x="0"
       off_y="0"
       off_z="0"
        role="0"
        pitch="0"
        yaw="0"
       type="fixed"
       full_joint="false"
       material="body_mat"
       Gmaterial="Black"
        mu1="0.5"
        mu2="50.0"
        kp="100000000.0"
        kd="1.0"
 />

  <!-- off_x = half body + half tail-->
  <xacro:body
       name="tail"
       parent="main_body_link"
       length="0.22"
       width="0.10"
       height="0.02"
       mass="0.242"
       off_x="${-0.11-0.07}"
       off_y="0"
       off_z="0"
       role="0"
       pitch="0"
       yaw="0"
       type="fixed"
       full_joint="true"
       material="tail_mat"
       Gmaterial="Black"
       mu1="0.5"
       mu2="50.0"
       kp="100000000.0"
       kd="1.0"

 />

  <xacro:finn
       name="upper_finn"
       parent="main_body_link"
        reflect="1"
       radius="0.01"
       mass="0.050"
       off_x="${-0.07-0.165}"
       off_y="0"
       off_z="${0.055-0.01}"
       role="${PI/2}"
       pitch="0"
       yaw="0"
       type="fixed"
       material="u_sphere_mat"
       Gmaterial="Orange"
       mu1="0"
       mu2="0"
       kp="10000000"
       kd="1"
       maxVel="1000"
       minDepth="0"
       fdir1="0 0 1"
 />

  <xacro:finn
       name="lower_finn"
       parent="main_body_link"
       reflect="-1"
       radius="0.01"
       mass="0.050"
       off_x="${-0.07-0.165}"
       off_y="0"
       off_z="${0.055-0.01}"
       role="${PI/2}"
       pitch="0"
       yaw="0"
       type="fixed"
       material="l_sphere_mat"
       Gmaterial="Blue"
       mu1="0"
       mu2="0"
       kp="10000000"
       kd="1"
       maxVel="1000"
       minDepth="0"
       fdir1="0 0 1"

 />

</robot>

wheel.urdf.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- The "wheel" macro defines a wheel. -->
  <xacro:macro name="wheel" params="name parent reflect length radius mass off_x off_y off_z role pitch yaw type material Gmaterial mu1 mu2 kp kd maxVel minDepth fdir1">

    <joint name="${parent}_to_${name}_wheel_joint" type="${type}">
      <parent link="${parent}"/>
      <child link="${name}_wheel_link"/>
      <origin rpy= "${role} ${pitch} ${yaw}" xyz="${off_x} ${reflect*off_y} ${off_z}"/>
      <axis xyz="0 0 1"/>      
      <dynamics damping="0.1" friction="100.0"/>
      <!-- <dynamics damping="1.0" friction="1.0" spring_stiffness="1" /> -->
      <!-- <limit effort="100" velocity="100" /> -->
    </joint>

    <link name="${name}_wheel_link">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="${material}"/>
      </visual>

      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
      </collision>

      <xacro:wheel_inertial
         rad="${radius}" height="${length}" mass="${mass}">
          <origin rpy="${PI/2} 0 0" xyz="0 0 0"/>
      </xacro:wheel_inertial>
    </link>

    <gazebo reference="${name}_wheel_link">
      <!-- <material>Gazebo/Black</material> -->
      <material>Gazebo/${Gmaterial}</material>
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
      <kp>${kp}</kp>
      <kd>${kd}</kd>
      <maxVel>${maxVel}</maxVel>
      <minDepth>${minDepth}</minDepth>
      <fdir1>${fdir1}</fdir1>
    </gazebo>

    <gazebo reference="${parent}_to_${name}_wheel_joint}" >
      <!--
     <kp>100000000.0</kp>
     <kd>1.0</kd>
     -->
      <kp>${kp}</kp>
      <kd>${kd}</kd>
    </gazebo>

    <!--
   <transmission name="${parent}_to_${name}_wheel_trans">
     <type>pr2_mechanism_model/SimpleTransmission</type>
     <joint name="${parent}_to_${name}_wheel_joint" />
     <actuator name="${name}_wheel_motor">
       <hardwareInterface>EffortJointInterface</hardwareInterface>
       <mechanicalReduction>${reflect * 624/35 * 80/19}</mechanicalReduction>
     </actuator>
   </transmission>
   -->

  </xacro:macro>

  <!-- off_y = half body + half wheel + 1cm gab between. -->
  <xacro:wheel  
       name="left"  
       reflect="1"  
       parent="main_body_link"  
       length="0.04"  
       radius="0.12"  
       mass="0.242"  
       off_x="0"  
       off_y="${0.11+0.02+0.01}"
       off_z="0"
        role="-${PI/2}"
        pitch="0"
        yaw="0"  
       type="continuous"
       material="l_wheel_mat"  
       Gmaterial="Green"
       mu1="100"
       mu2="100"
       kp="100000000"
       kd="1"
        maxVel="100000000"
        minDepth="0"
        fdir1="0 0 1"
 />

  <xacro:wheel  
       name="right"  
       reflect="-1"  
       parent="main_body_link"  
       length="0.04"  
       radius="0.12"  
       mass="0.242"  
       off_x="0"  
       off_y="${0.11+0.02+0.01}"  
       off_z="0"  
       role="-${PI/2}"
       pitch="0"
       yaw="0"
       type="continuous"  
       material="r_wheel_mat"  
       Gmaterial="Red"
       mu1="100"
       mu2="100"
       kp="100000000"
       kd="1"
        maxVel="100000000"
       minDepth="0"
       fdir1="0 0 1"
 />

  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <alwaysOn>true</alwaysOn>
      <!--updateRate>${update_rate}</updateRate-->
      <leftJoint>main_body_link_to_right_wheel_joint</leftJoint>
      <rightJoint>main_body_link_to_left_wheel_joint</rightJoint>
      <wheelSeparation>0.2800</wheelSeparation>
      <wheelDiameter>0.2400</wheelDiameter>
      <torque>20</torque>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_footprint</robotBaseFrame>
    </plugin>
  </gazebo>

</robot>

common.urdf.xacro:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Degree-to-radian conversions -->
  <xacro:property name="degrees_45" value="0.785398163"/>
  <xacro:property name="degrees_90" value="1.57079633"/>
  <xacro:property name="PI" value="3.1415"/>

  <!-- Some predefined materials/colors -->
  <material name="l_wheel_mat">
    <color rgba="0 1 0 1"/>
  </material>
  <material name="r_wheel_mat">
    <color rgba="1 0 0 1"/>
  </material>
  <material name="tail_mat">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="body_mat">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="tire_mat">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="u_sphere_mat">
    <color rgba="1 0.7 0 1"/>
  </material>
  <material name="l_sphere_mat">
    <color rgba="0 0 1 1"/>
  </material>

  <!-- Null inertial element. This is needed to make the model work with
      Gazebo. -->
  <xacro:macro name="null_inertial">
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
  </xacro:macro>

  <!-- Inertia of a solid cuboid. Width is measured along the x axis, depth
      along the y axis, and height along the z axis. -->
  <xacro:macro name="solid_cuboid_inertial"
              params="width depth height mass *origin">
    <inertial>
      <xacro:insert_block name="origin"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass * (depth * depth + height * height) / 12}"
              ixy="0" ixz="0"
              iyy="${mass * (width * width + height * height) / 12}"
              iyz="0"
              izz="${mass * (width * width + depth * depth) / 12}"/>
    </inertial>
  </xacro:macro>


  <!-- Inertia of wheel. Height is measured along the z axis,
      which is the wheels's axis. -->
  <xacro:macro name="wheel_inertial"
              params="rad height mass *origin">
    <inertial>
      <xacro:insert_block name="origin"/>
      <mass value="${mass}"/>
      <inertia ixx="${mass * (3 * rad * rad + height * height)/12}"
              ixy="0" ixz="0"
              iyy="${mass * (3 * rad * rad + height * height)/12}"
              iyz="0"
              izz="${mass * rad * rad / 2}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="sphere_inertial" params="radius mass *origin">
    <inertial>
      <mass value="${mass}" />
      <insert_block name="origin" />
      <inertia ixx="${2.0 / 5.0 * mass * radius * radius}" ixy="0.0" ixz="0.0"
              iyy="${2.0 / 5.0 * mass * radius * radius}" iyz="0.0"
              izz="${2.0 / 5.0 * mass * radius * radius}" />
    </inertial>
  </xacro:macro>

</robot>

robot.urdf.xacro:

<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find description)/urdf/robot/wheel.urdf.xacro" />
  <xacro:include filename="$(find description)/urdf/robot/body.urdf.xacro" />
  <xacro:include filename="$(find description)/urdf/robot/common.urdf.xacro" />

  <!-- base_link must have geometry so that its axes can be displayed in
      rviz. -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="body_mat"/>
    </visual>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Grey</material>
  </gazebo>

</robot>

Could anybody help me with that?

My second problem:

When I drive backwards the rover stutter when the sphere-slider hits the ground after the tail lifted up into the air. The sphere has a friction of mu1=mu2=0 and a stiffness kp=1 mil.

I think this Video will explain it.

Would be really nice if anybody could help me with that too.

Thank you and best regards,

F4B1