Robotics StackExchange | Archived questions

Why does my differential drive robot wheels move as if flushed in a toilet?

Hi. I'm new to ROS and Gazebo simulation. I tried to create a differential drive robot for various experiments such sa SLAM etc.

My problem right now is that my driven wheels drift as if they're not attached to the main link and whenever I run teleop, they rotate and move as if they were flushed in a toilet and the wheels suddenly stick together and continue to rotate.

Here is a link to a video I took showing my problem. https://www.youtube.com/watch?v=0gAoxdEr034

Here is a link where I uploaded my files as pasting them here would make this post really long. I would really appreciate it if you would have a look. https://github.com/jcjmagno/test_robot

Here is a snippet of the urdf code:

<link name="base_link">
</link>

<joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="chassis"/>
</joint>

<link name="chassis">

    <inertial>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <mass value="25"/>
        <inertia ixx="3.002" ixy="0.0" ixz="0.0" iyy="4.011" iyz="0.0" izz="2.8465"/>
    </inertial>
    <visual name="chassis_visual">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <geometry>
            <box size="0.962 0.664 1" />
        </geometry>
    </visual>
    <collision name="chassis_collision">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <geometry>
            <box size="0.962 0.664 1" />
        </geometry>
    </collision>
</link>

<link name="front_left_wheel">
    <inertial>
        <origin xyz="0.3 0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
   <visual name="front_left_wheel_visual">
        <origin xyz="0.3 0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="front_left_wheel_collision">
        <origin xyz="0.3 0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<link name="front_right_wheel">
    <inertial>
        <origin xyz="0.3 -0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
    <visual name="front_right_wheel_visual">
        <origin xyz="0.3 -0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="front_right_wheel_collision">
        <origin xyz="0.3 -0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<link name="rear_left_wheel">
    <inertial>
        <origin xyz="-0.3 0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
    <visual name="rear_left_wheel_visual">
        <origin xyz="-0.3 0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="rear_left_wheel_collision">
        <origin xyz="-0.3 0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<link name="rear_right_wheel">
    <inertial>
        <origin xyz="-0.3 -0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <mass value="5"/>
        <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
    </inertial>
    <visual name="rear_right_wheel_visual">
        <origin xyz="-0.3 -0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </visual>
    <collision name="rear_right_wheel_collision">
        <origin xyz="-0.3 -0.232 -0.525" rpy="0.0 1.5707 1.5707"/>
        <geometry>
            <cylinder radius="0.1" length="0.1"/>
        </geometry>
    </collision>
</link>

<joint name="front_left_wheel_hinge" type="continuous">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="front_left_wheel"/>
    <axis xyz="0.0 0.0 1.0"/>
    <limit effort="100" velocity="100"/>
</joint>

<joint name="front_right_wheel_hinge" type="continuous">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="front_right_wheel"/>
    <axis xyz="0.0 0.0 1.0"/>
    <limit effort="100" velocity="100"/>
</joint>

<joint name="rear_left_wheel_hinge" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="rear_left_wheel"/>
    <axis xyz="0.0 0.0 1.0"/>
    <dynamics friction="0.0" />
    <limit lower="0.0" upper="0.0" effort="100" velocity="100"/>
</joint>

<joint name="rear_right_wheel_hinge" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    <parent link="chassis"/>
    <child link="rear_right_wheel"/>
    <axis xyz="0.0 0.0 1.0"/>
    <dynamics friction="0.0" />
    <limit lower="0.0" upper="0.0" effort="100" velocity="100"/>
</joint>

<gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/test_robot</robotNamespace> -->
    </plugin>
</gazebo>

<transmission name="chassis_to_front_left_wheel_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor1">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="front_left_wheel_hinge">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<transmission name="chassis_to_front_right_wheel_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="motor2">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="front_right_wheel_hinge">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
</transmission>

<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
        <updateRate>20</updateRate>
        <leftJoint>front_left_wheel_hinge</leftJoint>
        <rightJoint>front_right_wheel_hinge</rightJoint>
        <wheelSeparation>0.464</wheelSeparation>
        <wheelDiameter>0.2</wheelDiameter>
        <wheelAcceleration>1.0</wheelAcceleration>
        <wheelTorque>20</wheelTorque>
        <commandTopic>/test_robot/diff_drive_controller/cmd_vel</commandTopic>
        <odometryTopic>/test_robot/diff_drive_controller/odom</odometryTopic>
        <odometryFrame>/test_robot/diff_drive_controller/odom</odometryFrame>
        <robotBaseFrame>base_link</robotBaseFrame>
        <odometrySource>1</odometrySource>
        <publishWheelTF>true</publishWheelTF>
        <publishOdom>true</publishOdom>
        <publishWheelJointState>true</publishWheelJointState>
        <legacyMode>false</legacyMode>
    </plugin>
</gazebo>

Here is a snippet of the diff_drive.yaml code:

testrobot: jointstatecontroller: type: jointstatecontroller/JointStateController publishrate: 50

diffdrivecontroller: type : "diffdrivecontroller/DiffDriveController" leftwheel : 'frontleftwheelhinge' rightwheel : 'frontrightwheelhinge' publishrate: 50.0 # default: 50 posecovariancediagonal : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] twistcovariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter
wheel_separation : 0.464
wheel_radius : 0.1

# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier    : 1.0 # default: 1.0

# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 0.25

# Base frame_id
base_frame_id: chassis #default: base_link

# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
  x:
    has_velocity_limits    : true
    max_velocity           : 1.0  # m/s
    min_velocity           : -0.5 # m/s
    has_acceleration_limits: true
    max_acceleration       : 0.8  # m/s^2
    min_acceleration       : -0.4 # m/s^2
    has_jerk_limits        : true
    max_jerk               : 5.0  # m/s^3
angular:
  z:
    has_velocity_limits    : true
    max_velocity           : 1.7  # rad/s
    has_acceleration_limits: true
    max_acceleration       : 1.5  # rad/s^2
    has_jerk_limits        : true
    max_jerk               : 2.5  # rad/s^3

Tried on: Ubuntu 18.04 LTS, ROS Melodic, Gazebo 9 Ubuntu 20.04 LTS, ROS Noetic, Gazebo 11

Asked by jcjmagno on 2020-12-14 02:14:53 UTC

Comments

Answers

While I don't have a straightforward answer to urdf, but we have diff_drive demo available in sdf used in Ignition. I believe Gazebo supports both urdf and sdf, so if you need a reference to compare against, it might be helpful to take a look here https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo4/examples/worlds/diff_drive.sdf and this is the demo plugin used in Ignition https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo4/src/systems/diff_drive

Hope this helps

Asked by claireyywang on 2020-12-18 14:12:21 UTC

Comments