Hi all,

I wrote a basic URDF model for a robot with two wheels at the back and one caster wheel at the front. Then, I am using "turtlesim teleop" to control the robot using the keyboard. But the robot is not behaving as expected while turning and going in reverse, please see the video attached. While turning, the motion is not smooth whereas while reversing, it is behaving really strange. I have tried changing the values of different parameters (including friction, etc.) but with no success. In the video, the robot first goes straight, then turns right, then turns left, then goes straight and then reverse. The link to the video: https://www.youtube.com/watch?v=JOSOXImbLBo&feature=youtu.be

The relevant code:

drsim.xacro

<?xml version="1.0"?>

<robot name="drsim" xmlns:xacro="http://www.ros.org/wiki/xacro">


<xacro:property name="PI" value="3.1415926535897931"/>

<xacro:property name="chassisHeight" value="0.1"/>
<xacro:property name="chassisLength" value="0.4"/>
<xacro:property name="chassisWidth" value="0.2"/>
<xacro:property name="chassisMass" value="50"/>

<xacro:property name="casterRadius" value="0.05"/>
<xacro:property name="casterMass" value="5"/>

<xacro:property name="wheelWidth" value="0.05"/>
<xacro:property name="wheelRadius" value="0.1"/>
<xacro:property name="wheelPos" value="0.1"/>
<xacro:property name="wheelMass" value="5"/>

<xacro:include filename="$(find drsim_description)/urdf/drsim.gazebo" />
<xacro:include filename="$(find drsim_description)/urdf/materials.xacro" />
<xacro:include filename="$(find drsim_description)/urdf/macros.xacro" />

<link name='base_link'>
    <collision>
        <origin xyz="0 0 ${2*casterRadius + chassisHeight/2}" rpy="0 0 0" />
        <geometry>
            <box size="${chassisLength} ${chassisWidth} ${chassisHeight}" />
        </geometry>
    </collision>
    <visual>
        <origin xyz="0 0 ${2*casterRadius + chassisHeight/2}" rpy="0 0 0" />
        <geometry>
            <box size="${chassisLength} ${chassisWidth} ${chassisHeight}" />
        </geometry>
        <material name="orange" />
    </visual>
</link>

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

<link name="caster_wheel">
    <collision>
        <origin xyz="${chassisLength/2 - casterRadius} 0 ${casterRadius}" rpy="0 0 0"/>
        <geometry>
            <sphere radius="${casterRadius}" />
        </geometry>
    </collision>
    <visual>
        <origin xyz="${chassisLength/2 - casterRadius} 0 ${casterRadius}" rpy="0 0 0"/>
        <geometry>
            <sphere radius="${casterRadius}" />
        </geometry>
        <material name="red" />
    </visual>
    <inertial>
        <origin xyz="${chassisLength/2 - casterRadius} 0 ${casterRadius}" rpy="0 0 0"/>
        <mass value="${casterMass}"/>
        <sphere_inertia m="${casterMass}" r="${casterRadius}"/>
    </inertial>
</link>

<wheel lr="left" tY="1"/>
<wheel lr="right" tY="-1"/>

</robot>

macros.xacro

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

<macro name="box_inertia" params="m x y z">
<inertia  ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
  iyy="${m*(x*x+z*z)/12}" iyz = "0"
  izz="${m*(x*x+z*z)/12}"
/>
</macro>

<macro name="sphere_inertia" params="m r">
<inertia  ixx="${2*m*r*r/5}" ixy = "0" ixz = "0"
  iyy="${2*m*r*r/5}" iyz = "0"
  izz="${2*m*r*r/5}"
/>
</macro>

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


<macro name="wheel" params="lr tY">

<link name="${lr}_wheel">
<collision>
  <origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
  <geometry>
    <cylinder length="${wheelWidth}" radius="${wheelRadius}"/>
  </geometry>
</collision>

<visual>
  <origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
  <geometry>
    <cylinder length="${wheelWidth}" radius="${wheelRadius}"/>
  </geometry>
  <material name="black"/>
</visual>

<inertial>
  <origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
  <mass value="${wheelMass}"/>
  <cylinder_inertia m="${wheelMass}" r="${wheelRadius}" h="${wheelWidth}"/>
</inertial>
</link>

<gazebo reference="${lr}_wheel">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp  value="1000000.0" />
<kd  value="1.0" />
<fdir1 value="0 1 0"/>
<material>Gazebo/Black</material>
</gazebo>


<joint name="${lr}_wheel_hinge" type="continuous">
<parent link="base_link"/>
<child link="${lr}_wheel"/>
<origin xyz="${wheelPos - chassisLength/2} ${tY*wheelWidth/2+tY*chassisWidth/2} ${wheelRadius}" rpy="0 0 0" />
<axis xyz="0 1 0" rpy="0 0 0" />
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>


<transmission name="${lr}_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${lr}_wheel_hinge">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${lr}Motor">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
  <mechanicalReduction>10</mechanicalReduction>
</actuator>
</transmission>

</macro>
</robot>

drsim.gazebo

<?xml version="1.0"?>
<robot>

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

<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>50</updateRate>
        <leftJoint>left_wheel_hinge</leftJoint>
        <rightJoint>right_wheel_hinge</rightJoint>
        <wheelSeparation>${chassisWidth+wheelWidth}</wheelSeparation>
        <wheelDiameter>${2*wheelRadius}</wheelDiameter>
        <torque>20</torque>
        <commandTopic>/cmd_vel</commandTopic>
        <odometryTopic>/odom</odometryTopic>
        <odometryFrame>odom</odometryFrame>
        <robotBaseFrame>base_link</robotBaseFrame>
    </plugin>
</gazebo>

<gazebo reference="base_link">
    <material>Gazebo/Orange</material>
</gazebo>

<gazebo reference="caster_wheel">
    <mu1>0</mu1>
    <mu2>0</mu2>
    <material>Gazebo/Red</material>
</gazebo>   

</robot>

Any guidance will be appreciated.
Thanks in advance.

Naman Kumar