Home | Tutorials | Wiki | Issues
Ask Your Question
0

Simple heavy differential drive Robot with urdf

asked 2015-04-21 00:03:04 -0500

AReimann gravatar image

updated 2015-04-21 20:28:45 -0500

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2015-05-29 18:53:00 -0500

hsu gravatar image

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
edit flag offensive delete link more
0

answered 2015-04-29 11:36:29 -0500

nkoenig gravatar image

I ran your model on gazebo 6, and I had to make one key change. The wheel links need a <max_vel> 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 ...
(more)
edit flag offensive delete link more

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

AReimann gravatar imageAReimann ( 2015-05-07 03:22:14 -0500 )edit

After deleting the springstiffness, springreference and useparentmodelframe 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/WJle0KUevM That's not dependent on the mass though

AReimann gravatar imageAReimann ( 2015-05-07 03:32:10 -0500 )edit

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.

AReimann gravatar imageAReimann ( 2015-05-14 03:45:12 -0500 )edit

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

AReimann gravatar imageAReimann ( 2015-05-29 01:46:00 -0500 )edit
Login/Signup to Answer

Question Tools

Stats

Asked: 2015-04-21 00:03:04 -0500

Seen: 2,966 times

Last updated: May 29 '15