DiffDrive robot wobbling up and down when moving, not smooth

I have been following the documentation to make a simple diff drive robot with 2 back wheels and one caster wheel but as seen from the video the robot keeps wobbling up and down

as seen from the video it’s just going up and down not a steady or smooth at all
From searching i found people usually fix it with adding friction to the caster wheel, it didn’t’ fix it for me…

This is the code for the caster wheel

<!-- CASTER -->
    <link name="caster_wheel">
        <visual>
            <geometry>
                <sphere radius="${caster_radius}" />
            </geometry>
            <material name="black" />
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        </visual>
        <collision>
            <geometry>
                <sphere radius="${caster_radius}" />
            </geometry>

            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />

            <surface>
                <friction>
                    <ode>
                        <mu>0.4</mu>
                        <mu2>0.4</mu2>
                        <slip1>1.0</slip1>
                        <slip2>1.0</slip2>
                    </ode>
                </friction>
            </surface>
        </collision>

        <xacro:solid_sphere_inertial m="${caster_mass}" r="${caster_radius}" />
    </link>

    <joint name="base_caster_wheel_joint" type="fixed">
        <parent link="base_link" />
        <child link="caster_wheel" />

        <origin xyz="${base_length * 1/4} 0.0 -${caster_radius + base_height / 2}" rpy="0.0 0.0 0.0" />
    </joint>

Here is the sphere inertial

    <xacro:macro name="solid_sphere_inertial" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2/3 * m * r * r}" ixy="0.0" ixz="0.0" iyy="${2/3 * m * r * r}" iyz="0.0"
                izz="${2/3 * m * r * r}" />
        </inertial>
    </xacro:macro>

those are my simulated robot dimensions and masses

    <xacro:property name="base_length" value="0.6" />
    <xacro:property name="base_width" value="0.3" />
    <xacro:property name="base_height" value="0.1" />
    <xacro:property name="base_mass" value="0.5" />

    <xacro:property name="wheel_radius" value="0.07" />
    <xacro:property name="wheel_length" value="0.04" />
    <xacro:property name="wheel_mass" value="0.1" />


    <xacro:property name="caster_radius" value="0.03" />
    <xacro:property name="caster_mass" value="0.05" />

This is extra info of my robot base link and wheels, i’m really sorry if i’m missing something obvious there i’m quite new to gazebo and i have been really struggling with this one for a while

<!-- BASE LINKS -->
    <link name="base_footprint" />

    <link name="base_link">
        <visual>
            <geometry>
                <box size="${base_length} ${base_width} ${base_height}" />
            </geometry>
            <material name="green" />
            <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
        </visual>
        <collision>
            <geometry>
                <box size="${base_length} ${base_width} ${base_height}" />
            </geometry>
        </collision>
        <xacro:box_inertial m="${base_mass}" l="${base_length}" w="${base_width}" h="${base_height}" />
    </link>

    <joint name="base_footprint_base_link_joint" type="fixed">
        <parent link="base_footprint" />
        <child link="base_link" />
        <origin xyz="0 0.0 ${base_height / 2}" rpy="0.0 0.0 0.0" />
    </joint>

    <!-- WHEELS -->
    <xacro:macro name="wheel" params="prefix direction_sign">
        <link name="${prefix}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>
                <origin xyz="0.0 0.0 0.0" rpy="${pi / 2} 0.0 0.0" />
            </visual>

            <collision>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            </collision>

            <xacro:cylinder_inertial m="${wheel_mass}" r="${wheel_radius}" l="${wheel_length}" />
        </link>

        <joint name="base_${prefix}_wheel_joint" type="continuous">
            <parent link="base_link" />
            <child link="${prefix}_wheel" />

            <axis xyz="0.0 1 0.0" />
            <origin
                xyz="-${base_length * 1/4} ${direction_sign}${base_width / 2 + wheel_length / 2} ${-base_height / 2}"
                rpy="0.0 0.0 0.0" />
        </joint>
    </xacro:macro>

    <xacro:wheel prefix="left" direction_sign="-" />
    <xacro:wheel prefix="right" direction_sign="+" />

Visualize the collisions. It is possible your cylinders have wrong orientation.

The wheel visual has:

<origin xyz="0.0 0.0 0.0" rpy="${pi / 2} 0.0 0.0" />

While the collision element has

<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />

So it looks like it is missing the rotation.

1 Like

I really can’t thank you enough i didn’t know collision could cause this i thought it’s related to physics or missing friction property
Really thank you so much :pray:

1 Like