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

URDF - wheels joints are stuck to base_link

asked 2017-03-12 09:42:28 -0500

Moneyball gravatar image

updated 2017-03-13 00:36:40 -0500

After spending so many hours figuring things out, I finally got to make a two wheeled robot that gets controlled with diff_drive_controller using ROS. However, whenever i publish cmd_vel like the following:

rostopic pub /diff_drive_controller/cmd_vel geometry_msgs/Twist -- '[1.0, 0, 0]' '[0.0, 0.0, 1.2]'

the robot moves in a weird way: the base_link seems to be fixed to the wheels. Whenever the wheels move, base_link moves with the wheels. What I want is the base_link to stay still and the wheels to rotate.

Here is my urdf of the two wheeled robot:

<robot name="asst1"> <gazebo> <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> </plugin> </gazebo>

<link name="base_link">
    <collision name='collision'>
        <origin xyz="0 0 0.1" rpy="0 0 0" />
        <geometry>
            <box size=".3 .3 .05"/>
        </geometry>
    </collision>

   <visual name='visual'>
       <origin xyz="0 0 0.1" rpy="0 0 0" />
       <geometry>
           <box size=".3 .3 .05"/>
       </geometry>
   </visual>

   <inertial>
       <origin xyz="-0.1 0 0.1" rpy="0 0 0" />
       <mass value="5"/>
       <inertia
           ixx="0.03854" ixy="0.0" ixz="0.0"
           iyy="0.075" iyz="0.0"
           izz="0.03854"/>
    </inertial>
</link>


<link name="left_wheel">
    <collision name="collision">
        <origin xyz="0 0 0" rpy="0 1.5708 1.5708" />
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
    </collision>

    <visual name="visual">
        <origin xyz="0 0 0" rpy="0 1.5708 1.5708" />
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
    </visual>

    <inertial>
        <mass value="1"/>
        <inertia
            ixx="0.002708" ixy="0.0" ixz="0.0"
            iyy="0.002708" iyz="0.0"
            izz="0.005"/>
    </inertial>
</link>


<link name="right_wheel">
    <collision name="collision">
        <origin xyz="0 0 0" rpy="0 1.5708 1.5708" />
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
    </collision>

    <visual name="visual">
        <origin xyz="0 0 0" rpy="0 1.5708 1.5708" />
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
    </visual>

    <inertial>
        <mass value="1"/>
        <inertia
            ixx="0.002708" ixy="0.0" ixz="0.0"
            iyy="0.002708" iyz="0.0"
            izz="0.005"/>
        </inertial>
</link>

<gazebo reference="left_wheel">
    <mu1 value="10000.0" />
    <mu2 value="10000.0" />
    <fdir1>0 1 0</fdir1>
    <minDepth>0.01</minDepth>
</gazebo>


<gazebo reference="right_wheel">
    <mu1 value="10000.0" />
    <mu2 value="10000.0" />
    <fdir1>0 1 0</fdir1>
    <minDepth>0.01</minDepth>
</gazebo>

<joint type="continuous" name="left_joint">
    <child link="left_wheel">left_wheel</child>
    <parent link="base_link">base_link</parent>
    <axis xyz="0 1 0"/>
    <origin xyz="0 -0.175 0.1" rpy="0 0 0"/>
    <!--<limit effort="100" velocity="10.0"/> --> 
</joint>


<joint type="continuous" name="right_joint">
    <child link="right_wheel">right_wheel</child>
    <parent link="base_link">base_link</parent>
    <axis xyz="0 1 0"/>
    <origin xyz="0 0.175 0.1" rpy="0 0 0"/>
    <!--<limit effort="100" velocity="10.0"/> --> 
</joint>


<transmission name="right_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_joint">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_motor"/>
    <mechanicalReduction>1</mechanicalReduction>
    <motorTorqueConstant>1</motorTorqueConstant>
</transmission>

<transmission name="left_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_joint">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_motor ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-03-12 10:31:20 -0500

gudjon gravatar image

updated 2017-03-12 21:15:00 -0500

I'm new to this myself, but I resently got DiffDriveController to work on my robot. If I understand your problem correctly, I think you need to fix the mass / inertia. From what I have read, the identity matrix is a bad default for the moment of inertia matrix.

I use these macros in my model:

  <xacro: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}" /> 
  </xacro:macro>

  <xacro:macro name="box_inertia" params="m w h d">
          <inertia
            ixx="${m / 12.0 * (d*d + h*h)}" ixy="0.0" ixz="0.0"
            iyy="${m / 12.0 * (w*w + h*h)}" iyz="0.0"
            izz="${m / 12.0 * (w*w + d*d)}"/>
  </xacro:macro>

Make sure you configure the friction of your wheels so they don't slip. Here's a sample from my model. You will probably have to play with the values, some of them may be overkill.

  <gazebo reference="wheel_FL">
    <mu1 value="100000.0"/>
    <mu2 value="100000.0"/>
    <fdir1>0 1 0</fdir1>
    <minDepth>0.01</minDepth>
    <slip1>0</slip1>
    <slip2>0</slip2>
  </gazebo>
edit flag offensive delete link more

Comments

Hi thank you. I tried it with your equation and it seems to move better than before. But the movement is still a bit jerky. One quick question: what is the mass of your base_link and wheels?

Moneyball gravatar image Moneyball  ( 2017-03-12 20:05:33 -0500 )edit

Also in the yaml code, what exactly are pose_covariance_diagonal and twist_covariance diagonal? Will they affect the way my robot moves?

Moneyball gravatar image Moneyball  ( 2017-03-12 20:09:18 -0500 )edit

My vehicle is still a work in progress, but the mass is set very high (300+ for the base, 10 for each wheel), also it has 4 wheels.

Does your vehicle have a third contact to the ground, like a caster wheel, or does it stand on two wheels like an inverted pendulum?

gudjon gravatar image gudjon  ( 2017-03-12 20:57:18 -0500 )edit

As far as I know the covariance has nothing to do with the driving properties, it's only used for the odometry.

Another thing you could look into is the friction of the tires. To make sure their not slipping or bouncing. I'll update my answer to include a sample.

gudjon gravatar image gudjon  ( 2017-03-12 21:07:05 -0500 )edit

I don't have caster wheel, just two wheels on the side of the base_link. How do I change the friction? If I never specified it, would it default to 0 friction?

Moneyball gravatar image Moneyball  ( 2017-03-12 21:19:28 -0500 )edit

Just saw your updated post: ill go check to see if friction will do the trick

Moneyball gravatar image Moneyball  ( 2017-03-12 21:27:22 -0500 )edit

when i added <slip>, parseURDF failed so i only used mu1, mu2, fdir, and mindepth. I forgot the -r flag for rostopic pub. Now everything works! Thank you

Moneyball gravatar image Moneyball  ( 2017-03-13 00:32:46 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2017-03-12 09:42:28 -0500

Seen: 1,862 times

Last updated: Mar 13 '17