ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Model bounces in reaction to teleop_twist_keyboard input

asked 2020-12-18 19:56:29 -0500

sisko gravatar image

updated 2021-04-24 02:53:19 -0500

miura gravatar image

I have my robot model spawned into a gazebo world and I am attempting to drive it around using teleop.

When I push i to innitiate movement the robot flips up on it's rear wheels and almost flips over. Increasing the speed also creates unpredictable reactions. Stopping the robot also creates what I can only describe as a "bouncing effect". I inserted a screenshot below:

image description

The image above shows the robot flipping and bouncing after I pushed the stop (k) button. It's even worse when I try getting the robot to turn left or right.

I'm looking for suggestions on what could be causing this behaviour and how I can fix it, please.

*Update: *

URDF code added (with the exception of the ouster_description package lidar, inthe interest of preserving space) below as requested:

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

  <!-- body -->
  <xacro:property name="chasis_length" value="1.0"/>
  <xacro:property name="chasis_heigth" value="0.45"/>
  <xacro:property name="chasis_width" value="0.5"/>

  <link name="chasis">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <mass value="1.0"/>
      <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
    </inertial>
    <visual name="">
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="${chasis_length} ${chasis_heigth} ${chasis_width}"/>
      </geometry>
      <material name="">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="${chasis_length} ${chasis_heigth} ${chasis_width}"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference='chasis'>
    <material>Gazebo/Orange</material>
  </gazebo>

  <!-- WHEELS -->
  <xacro:property name="wheel_radius" value="0.1"/>
  <xacro:property name="wheel_length" value="0.1"/>
  <xacro:property name="wheel_rpy" value="1.57075 0 0"/>

  <xacro:macro name="wheel" params="side position flip alt">
    <xacro:property name="wheel_xyz" value="${alt * 0.3} ${flip * 0.2} -0.25"/>

    <link name="wheel_${side}_${position}">
      <inertial>
        <mass value="1.0"/>
        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
      </inertial>
      <visual name="">
        <origin xyz="0 0 0" rpy="${wheel_rpy}"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
        </geometry>
        <material name="">
          <color rgba="1.0 0.0 0.0 1.0"/>
          <texture filename=""/>
        </material>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="${wheel_rpy}"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
        </geometry>
      </collision>
    </link>

    <gazebo reference='"wheel_${side}_${position}'>
      <material>Gazebo/Red</material>
    </gazebo>

    <joint name="body_wheel_${side}_${position}_joint" type="continuous">
      <parent link="chasis"/>
      <child link="wheel_${side}_${position}"/>
      <limit lower ...
(more)
edit retag flag offensive close merge delete

Comments

Are the wheels spinning the way you expect for forward, backward, and turning motions? You may check by making your robot "float" with a fixed joint between world and base_link, so physics doesn't get in the way as suggested in this Gazebo answer. If that works, then the issue may be your physical parameters (inertia, friction, etc.). Either way, you should post your URDF file(s) for further troubleshooting.

tryan gravatar image tryan  ( 2020-12-21 09:25:30 -0500 )edit

@tryan: Hello again :-)

I added my urdf code as you requested.

Regarding the spinning movements of the wheels, I am certain they are moving in the right and required directions. I can get the model to move without issue if I reduce the to almost zero. That is particularly important when the robot is starting from stand still. Increasing the speed tends to have the "galloping horse" effect where the robot lifts up on it's rear wheels, comes down on all four wheels and then picks up speed.

sisko gravatar image sisko  ( 2020-12-21 14:37:27 -0500 )edit
1

Haha, hi, @sisko! I remember the tumbling wheels now :)

Looking a little closer at the URDF, it seems the wheels have significantly more inertia than the chassis (and too much for their mass value). The Husky's URDF provides an example of some real values ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467". You can use your mass value and the formula for a solid cylinder from Wikipedia's List of Moments of Inertia:

izz = 1/2 * m * r^2
ixx = iyy = 1/12 * m * (3 * r^2 + h^2)

where m is the mass, r is the radius, and h is the height (wheel width). You could also increase your chassis' mass (the Husky's is ~46 kg), and its inertia values should reflect its mass (see the formula linked above).

tryan gravatar image tryan  ( 2020-12-21 15:23:21 -0500 )edit

@tryan: Please excuse my ignorance because I am NOT a maths guy. Do you take a different formular from that Wiki page depending on the shape you wish to calculate for ? Am I correct in appling the formular you provided me to both the chassis and the wheels ? And lastly, can you recommend a video or article to help me understand the formulae ?

sisko gravatar image sisko  ( 2020-12-23 19:53:13 -0500 )edit

I submitted an answer in response as it allows more characters.

tryan gravatar image tryan  ( 2020-12-25 12:03:04 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-12-25 12:00:31 -0500

tryan gravatar image

Reposting as an answer for character limit.

Looking a little closer at the URDF, it seems the wheels have significantly more inertia than the chassis (and too much for their mass value). The Husky's URDF provides an example of some real values ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467". You can use your mass value and the formula for a solid cylinder from Wikipedia's List of Moments of Inertia:

izz = 1/2 * m * r^2
ixx = iyy = 1/12 * m * (3 * r^2 + h^2)

where m is the mass, r is the radius, and h is the height (wheel width). You could also increase your chassis' mass (the Husky's is ~46 kg), and its inertia values should reflect its mass (see the formula linked above).


Update

Do you take a different formular from that Wiki page depending on the shape you wish to calculate for ? Am I correct in appling the formular you provided me to both the chassis and the wheels ?

Yes, each shape has a specific formula. The one I posted is only for solid cylinders, like wheels, so no, you can't really use it for the chassis, too. The values may not be extremely far off, but it's better to use the correct formula for a given shape. Your chassis is a solid cuboid, which has

izz = 1/12 * m * (x^2 + y^2)
ixx = 1/12 * m * (y^2 + z^2)
iyy = 1/12 * m * (x^2 + z^2)

with the other values as 0. I should have noted above that the unmentioned inertia values are 0 also.

can you recommend a video or article to help me understand the formulae ?

Wikipedia's Moment of Inertia article is as good a place to start as any for a quick overview. Here's a Khan Academy video lesson on the topic that may be more helpful for understanding. Also, most general physics text books cover it.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-12-18 19:56:29 -0500

Seen: 411 times

Last updated: Dec 25 '20