Robotics StackExchange | Archived questions

Simple heavy differential drive Robot with urdf

Hey, I have a rather simple differential drive robot model. The problem is that even though the wheels are restricted to moving only around one axis they move weirdly as shown in this video.

The main body has a high weight.

How can I make this whole thing stable? How can I make the wheels fixed to the main body, except rotation?

Here is the xarco:

<?xml version="1.0" ?>
<robot name="gocart_v2_gazebo" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:property name="base_link"     value="base_link" />
  <xacro:property name="pi"            value="3.1415926535897931" />

  <!-- cannot put these properties within the macro tags -->
  <xacro:property name="gocart_bias"          value="0.400" />
  <xacro:property name="gocart_wheel_radius"  value="0.0751" />
  <xacro:property name="gocart_caster_radius" value="0.0375" />

  <macro name="cylinder_inertia" params="m r h">
    <inertia  ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
      iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
      izz="${m*r*r/2}"
    />
  </macro>

  <macro name="box_inertia" params="m x y z">
    <inertia  ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
      iyy="${m*(x*x+z*z)/12}" iyz = "0"
      izz="${m*(x*x+z*z)/12}"
    />
  </macro>

  <macro name="sphere_inertia" params="m r">
    <inertia  ixx="${2*m*r*r/5}" ixy = "0" ixz = "0"
      iyy="${2*m*r*r/5}" iyz = "0"
      izz="${2*m*r*r/5}"
    />
  </macro>

  <xacro:macro name="wheel" params="name parent length radius off_x off_y off_z type">
    <joint name="${name}" type="${type}">
      <parent link="${parent}"/>
      <child link="${name}_link"/>
      <origin rpy="0 0 0" xyz="${off_x} ${off_y} ${off_z}"/>
      <axis xyz="1 0 0"/>
      <dynamics damping="1.0" friction="1.0" spring_stiffness="1" />
    </joint>

    <link name="${name}_link">
      <visual>
        <geometry>
          <cylinder length="${length}" radius="${radius}"/>
        </geometry>
        <origin rpy="0 ${pi/2} 0" xyz="0 0 0"/>
      </visual>

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

      <inertial>
        <mass value="0.1" />
        <origin rpy="0 ${pi/2} 0" xyz="0 0 0"/>
        <cylinder_inertia m="0.1" r="${radius}" h="${length}"/>
      </inertial>
    </link>

    <gazebo reference="${name}">
<!--       <implicitSpringDamper>true</implicitSpringDamper> -->
    </gazebo>

    <gazebo reference="${name}_link">
      <mu1 value="100000.0" />
      <mu2 value="100000.0" />
      <kp value="100000.0" />
      <kd value="0.00001" />
      <maxVel value="1.0" />
      <minDepth value="0.0001" />
    </gazebo>    
  </xacro:macro>

  <xacro:macro name="caster" params="name parent radius off_x off_y off_z type">
    <joint name="${name}" type="${type}">
      <parent link="${parent}"/>
      <child link="${name}_link"/>
      <origin rpy="0 0 ${pi/2}" xyz="${off_x} ${off_y} ${off_z}"/>
      <dynamics damping="1.0" friction="0.0" />
    </joint>

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

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

      <inertial>
        <origin xyz="0 0 0" />
        <mass value="0.01" />
        <sphere_inertia m="0.01" r="${radius}"/>
      </inertial>
    </link>

    <gazebo reference="${name}_link">
      <mu1 value="0.0" />
      <mu2 value="0.0" />
      <kp value="1000000.0" />
      <kd value="1.0" />
      <maxVel value="1.0" />
      <minDepth value="0.0" />
    </gazebo>
  </xacro:macro>

  <link name="base_footprint"/>

  <joint name="base" type="fixed">
    <parent link="base_footprint"/>
    <child link="${base_link}"/>
    <origin xyz="0 0 0.075" rpy="0 0 0"/>
  </joint>

  <link name="${base_link}">
    <visual>
      <geometry>
        <box size="0.370 0.700 0.600"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.305"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.370 0.700  0.600"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.305"/>
    </collision>
    <inertial>
      <mass value="100.0" />
      <origin rpy="0 0 0" xyz="0 0 0.305"/>
      <box_inertia m="100.0" x="0.370" y="0.700" z="0.600"/>
    </inertial>
  </link>

  <xacro:wheel name="wheel_left"          parent="${base_link}" length="0.030" radius="${gocart_wheel_radius}"  off_x="${gocart_bias/2}"  off_y="0.0"  off_z="0.0000" type="continuous"/>
  <xacro:wheel name="wheel_right"         parent="${base_link}" length="0.030" radius="${gocart_wheel_radius}"  off_x="-${gocart_bias/2}"  off_y="0.0" off_z="0.0000" type="continuous"/>
<!--  <xacro:caster name="caster_front_left"  parent="${base_link}" radius="${gocart_caster_radius}" off_y="0.257"  off_x="0.095"  off_z="-0.0365" type="fixed"/>
  <xacro:caster name="caster_front_right" parent="${base_link}" radius="${gocart_caster_radius}" off_y="0.257"  off_x="-0.095" off_z="-0.0365" type="fixed"/>
  <xacro:caster name="caster_back_left"   parent="${base_link}" radius="${gocart_caster_radius}" off_y="-0.257" off_x="0.095"  off_z="-0.0375" type="fixed"/>
  <xacro:caster name="caster_back_right"  parent="${base_link}" radius="${gocart_caster_radius}" off_y="-0.257" off_x="-0.095" off_z="-0.0375" type="fixed"/>-->

  <gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <alwaysOn>true</alwaysOn>
      <robotNamespace>/</robotNamespace>
      <rosDebugLevel>Info</rosDebugLevel>
      <commandTopic>/gopher/commands/velocity</commandTopic>
      <odometryTopic>/odom</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <robotBaseFrame>base_link</robotBaseFrame>
      <publishWheelTF>true</publishWheelTF>
      <publishWheelJointState>true</publishWheelJointState>
      <wheelSeparation>0.4</wheelSeparation>
      <wheelDiameter>0.15</wheelDiameter>
      <wheelAcceleration>1.0</wheelAcceleration>
      <wheelTorque>200000</wheelTorque>
      <updateRate>30</updateRate>
      <odometrySource>world</odometrySource>
      <leftJoint>wheel_right</leftJoint>
      <rightJoint>wheel_left</rightJoint>
      <publishTf>1</publishTf>
    </plugin>
  </gazebo>

</robot>

Asked by AReimann on 2015-04-21 00:03:04 UTC

Comments

Answers

I ran your model on gazebo 6, and I had to make one key change. The wheel links need a around 100.

Here is the SDF I used:

<?xml version='1.0'?>
<sdf version='1.5'>
  <world name="default">
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <model name='gocart_v2_gazebo'>
      <link name='base_footprint'>
        <pose>0 0 0 0 -0 0</pose>
        <inertial>
          <pose>0 0 0.38 0 -0 0</pose>
          <mass>100</mass>
          <inertia>
            <ixx>7.08333</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>4.14083</iyy>
            <iyz>0</iyz>
            <izz>4.14083</izz>
          </inertia>
        </inertial>
        <collision name='base_footprint_collision'>
          <pose>0 0 0.38 0 -0 0</pose>
          <geometry>
            <box>
              <size>0.37 0.7 0.6</size>
            </box>
          </geometry>
        </collision>
        <visual name='base_footprint_visual'>
          <pose>0 0 0.38 0 -0 0</pose>
          <geometry>
            <box>
              <size>0.37 0.7 0.6</size>
            </box>
          </geometry>
        </visual>
      </link>
      <link name='wheel_left_link'>
        <pose>0.2 0 0.075 0 -0 0</pose>
        <inertial>
          <pose>0 0 0 3.14159 1.57079 3.14159</pose>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.0001485</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.0001485</iyy>
            <iyz>0</iyz>
            <izz>0.000282</izz>
          </inertia>
        </inertial>
        <collision name='wheel_left_link_collision'>
          <pose>0 0 0 3.14159 1.57079 3.14159</pose>
          <geometry>
            <cylinder>
              <length>0.03</length>
              <radius>0.0751</radius>
            </cylinder>
          </geometry>
          <surface>
            <contact>
              <ode>
                <kp>100000</kp>
                <kd>1e-05</kd>
                <max_vel>100</max_vel>
                <min_depth>0.0001</min_depth>
              </ode>
            </contact>
            <friction>
              <ode>
                <mu>100000</mu>
                <mu2>100000</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='wheel_left_link_visual'>
          <pose>0 0 0 3.14159 1.57079 3.14159</pose>
          <geometry>
            <cylinder>
              <length>0.03</length>
              <radius>0.0751</radius>
            </cylinder>
          </geometry>
        </visual>
        <gravity>1</gravity>
        <velocity_decay/>
        <self_collide>0</self_collide>
      </link>
      <joint name='wheel_left' type='revolute'>
        <child>wheel_left_link</child>
        <parent>base_footprint</parent>
        <axis>
          <xyz>1 0 0</xyz>
          <limit>
            <lower>-1e+16</lower>
            <upper>1e+16</upper>
          </limit>
          <dynamics>
            <damping>1</damping>
            <friction>1</friction>
            <spring_reference>0</spring_reference>
            <spring_stiffness>0</spring_stiffness>
          </dynamics>
          <use_parent_model_frame>1</use_parent_model_frame>
        </axis>
        <physics>
          <ode>
            <limit>
              <cfm>0</cfm>
              <erp>0.2</erp>
            </limit>
          </ode>
        </physics>
      </joint>
      <link name='wheel_right_link'>
        <pose>-0.2 0 0.075 0 -0 0</pose>
        <inertial>
          <pose>0 0 0 3.14159 1.57079 3.14159</pose>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.0001485</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>0.0001485</iyy>
            <iyz>0</iyz>
            <izz>0.000282</izz>
          </inertia>
        </inertial>
        <collision name='wheel_right_link_collision'>
          <pose>0 0 0 3.14159 1.57079 3.14159</pose>
          <geometry>
            <cylinder>
              <length>0.03</length>
              <radius>0.0751</radius>
            </cylinder>
          </geometry>
          <surface>
            <contact>
              <ode>
                <kp>100000</kp>
                <kd>1e-05</kd>
                <max_vel>100</max_vel>
                <min_depth>0.0001</min_depth>
              </ode>
            </contact>
            <friction>
              <ode>
                <mu>100000</mu>
                <mu2>100000</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name='wheel_right_link_visual'>
          <pose>0 0 0 3.14159 1.57079 3.14159</pose>
          <geometry>
            <cylinder>
              <length>0.03</length>
              <radius>0.0751</radius>
            </cylinder>
          </geometry>
        </visual>
        <gravity>1</gravity>
        <velocity_decay/>
        <self_collide>0</self_collide>
      </link>
      <joint name='wheel_right' type='revolute'>
        <child>wheel_right_link</child>
        <parent>base_footprint</parent>
        <axis>
          <xyz>1 0 0</xyz>
          <limit>
            <lower>-1e+16</lower>
            <upper>1e+16</upper>
          </limit>
          <dynamics>
            <damping>1</damping>
            <friction>1</friction>
            <spring_reference>0</spring_reference>
            <spring_stiffness>0</spring_stiffness>
          </dynamics>
          <use_parent_model_frame>1</use_parent_model_frame>
        </axis>
        <physics>
          <ode>
            <limit>
              <cfm>0</cfm>
              <erp>0.2</erp>
            </limit>
          </ode>
        </physics>
      </joint>
    </model>
  </world>
</sdf>

Asked by nkoenig on 2015-04-29 11:36:29 UTC

Comments

Gazebo version 2.2.5 complains about not being able to downgrade to sdf version 1.4 If i simply change the version number it fails to read elements: http://i.imgur.com/YFrkP9c.png

Asked by AReimann on 2015-05-07 03:22:14 UTC

After deleting the spring_stiffness, spring_reference and use_parent_model_frame elements it runs, but the robot wheels weirdly snap together after a few seconds or when it is lifted: http://i.imgur.com/9asksGG.png / video: http://youtu.be/WJle0_KUevM That's not dependent on the mass though

Asked by AReimann on 2015-05-07 03:32:10 UTC

Using Gazebo 5 and your original model, this happens when I move the robot up and let it fall a bit: https://youtu.be/VI2n5NJLfpw Same happens when I try to drive my model.

Asked by AReimann on 2015-05-14 03:45:12 UTC

Same problem in Gazebo 6. Do you have an idea how to solve this? It's a really basic model.

Asked by AReimann on 2015-05-29 01:46:00 UTC

Due to large inertia ratio, you'll have to either:

  1. increase iters parameter see this example snippet, here iters is set to 1000. You may want to start with order of magnitude test, try 100, 1000, 10000, etc.
  2. use inertia ratio reduction in ode. see this example
  3. use a different solver/physics engine. See this tutorial on profiles

Asked by hsu on 2015-05-29 18:53:00 UTC

Comments