Robotics StackExchange | Archived questions

revolute2 joints are not holding high mass links

On adding high mass in inertial to the chassis of utility cart model the chassis is dropping to ground as the joints to the wheels are of type revolute2. If I change them to revolute joints the chassis stays in position.

http://pastebin.com/PXY2JWSv

Am I missing something. I faced similar problem while adding 2 revolute joints in series too for a car's steering and wheel rotation. image description

Asked by jitrc on 2014-07-01 12:51:49 UTC

Comments

Answers

I was unable to reproduce the problem. I'm running Gazebo 3.1, and I used the following SDF file (which is your model minus the plugin):

<?xml version="1.0" ?>
<sdf version="1.4">
  <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="car">
      <link name="chassis">
        <pose>0 0 0.3 0 0 0</pose>
        <inertial>
          <mass>720.0</mass>
          <inertia>
            <ixx>140</ixx>
            <ixy>0.0</ixy><iyy>550</iyy>
            <ixz>0.0</ixz><iyz>0.0</iyz><izz>550</izz>
          </inertia>
          <pose>0.1 0 0.4  0 0 0</pose>
        </inertial>
        <collision name="collision">
          <geometry>
            <box><size>2.0 1.0 0.2</size></box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box><size>2.0 1.0 0.2</size></box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Green</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="front_left_wheel">
        <pose>0.8 0.6 0.3 0 1.5707 1.5707</pose>
        <collision name="collision">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Black</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="front_right_wheel">
        <pose>0.8 -0.6 0.3 0 1.5707 1.5707</pose>
        <collision name="collision">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Black</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="back_right_wheel">
        <pose>-0.8 -0.6 0.3 0 1.5707 1.5707</pose>
        <collision name="collision">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Black</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="back_left_wheel">
        <pose>-0.8 0.6 0.3 0 1.5707 1.5707</pose>
        <collision name="collision">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder><radius>0.3</radius><length>0.1</length></cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Black</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="gas_pedal">
        <pose>0.3 0.1 0.5 0 0 0</pose>
        <collision name="collision">
          <geometry>
            <box><size>0.1 0.1 0.1</size></box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box><size>0.1 0.1 0.1</size></box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Red</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="brake_pedal">
        <pose>0.3 0.25 0.5 0 0 0</pose>
        <collision name="collision">
          <geometry>
            <box><size>0.1 0.1 0.1</size></box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box><size>0.1 0.1 0.1</size></box>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Blue</name>
            </script>
          </material>
        </visual>
      </link>
      <link name="steering_wheel">
        <pose>0.3 0.2 1.0 0 1.5707 0</pose>
        <collision name="collision">
          <geometry>
            <cylinder><radius>0.1</radius><length>0.01</length></cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder><radius>0.1</radius><length>0.01</length></cylinder>
          </geometry>
          <material>
            <script>
              <uri>file://media/materials/scripts/gazebo.material</uri>
              <name>Gazebo/Blue</name>
            </script>
          </material>
        </visual>
      </link>
      <joint type="prismatic" name="gas_joint">
        <parent>chassis</parent>
        <child>gas_pedal</child>
        <axis>
          <xyz>1 0 0</xyz>
          <limit><lower>0</lower><upper>0.2</upper></limit>
        </axis>
      </joint>
      <joint type="prismatic" name="brake_joint">
        <parent>chassis</parent>
        <child>brake_pedal</child>
        <axis>
          <xyz>1 0 0</xyz>
          <limit><lower>0</lower><upper>0.2</upper></limit>
        </axis>
      </joint>
      <joint type="revolute" name="steering_joint">
        <parent>chassis</parent>
        <child>steering_wheel</child>
        <axis>
          <xyz>1 0 0</xyz>
          <limit><lower>-7.853</lower><upper>7.853</upper></limit>
        </axis>
      </joint>
      <joint type="revolute2" name="front_left_joint">
        <parent>chassis</parent>
        <child>front_left_wheel</child>
        <axis>
          <xyz>0 0 1</xyz>
          <limit>
            <lower>0</lower>
            <upper>0</upper>
          </limit>
        </axis>
        <axis2>
          <xyz>0 1 0</xyz>
        </axis2>
        <physics>
          <ode>
            <limit><cfm>0.0</cfm><erp>0.9</erp></limit>
          </ode>
        </physics>
      </joint>
      <joint type="revolute2" name="front_right_joint">
        <parent>chassis</parent>
        <child>front_right_wheel</child>
        <axis><xyz>0 0 1</xyz>
          <limit><lower>0</lower><upper>0</upper></limit>
        </axis>
        <axis2>
          <xyz>0 1 0</xyz>
        </axis2>
        <physics>
          <ode>
            <limit><cfm>0.0</cfm><erp>0.9</erp></limit>
          </ode>
        </physics>
      </joint>
      <joint type="revolute2" name="back_right_joint">
        <parent>chassis</parent>
        <child>back_right_wheel</child>
        <axis><xyz>0 0 1</xyz>
          <limit><lower>0</lower><upper>0</upper></limit>
        </axis>
        <axis2>
          <xyz>0 1 0</xyz>
        </axis2>
        <physics>
          <ode>
            <limit><cfm>0.0</cfm><erp>0.9</erp></limit>
          </ode>
        </physics>
      </joint>
      <joint type="revolute2" name="back_left_joint">
        <parent>chassis</parent>
        <child>back_left_wheel</child>
        <axis><xyz>0 0 1</xyz>
          <limit><lower>0</lower><upper>0</upper></limit>
        </axis>
        <axis2>
          <xyz>0 1 0</xyz>
        </axis2>
        <physics>
          <ode>
            <limit><cfm>0.0</cfm><erp>0.9</erp></limit>
          </ode>
        </physics>
      </joint>
    </model>
  </world>
</sdf>

Asked by nkoenig on 2014-07-01 17:18:19 UTC

Comments

Just checked now, the problem is fixed in Gazebo 3. I was using 1.9.5, the problem persists in 2.2 also. Thanks. I would try to add suspension to the car using prismatic joints

Asked by jitrc on 2014-07-02 00:59:59 UTC

Actually It is still not working in Gazebo 3.0. Gazebo choose a different sdf file as 3.0 supports sdf 1.5. I added inertia to that file also and the problem remains

Asked by jitrc on 2014-07-02 04:08:22 UTC

Take a look at the vehicle models (including one with suspension) in the following pull request: https://bitbucket.org/osrf/gazebo_models/pull-request/82/vehicle-models-with-steering-and-soft

Asked by scpeters on 2014-07-03 12:27:02 UTC

That would solve everything, thanks. I would also make our model and plugin available in public as soon as I get it working. Currently we are developing it at aurobots.com

Asked by jitrc on 2014-07-04 11:56:43 UTC

If you want to make your model publically available, please submit a pull request to https://bitbucket.org/osrf/gazebo_models. We love new models!

Asked by nkoenig on 2014-07-09 11:47:11 UTC