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

edit

Hi everybody,

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

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 ...