Robot Drifting in rviz

Question

Hello I have my custom made robot URDF which I a using to simulate the robot in rviz, I am publishing the odo data over TF for simulation.

But When I give velocity (fake) as vx =1, vy = -1 and angular velocity as 0, then robot actually drifts, ie. the robot is not moving as expected, it moves in a straight line with at an angle of 45 deg from the origin and even the orientation or heading of the robot doesn't match with the direction of the moment of the robot.

The urdf for my robot is as below:

  <link name="base_link">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>
    <visual>
      <geometry>
        <box size="0.6 0.35 0.15"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="white">
        <color rgba="0.2 1 0.3 1"/>
      </material>
    </visual>
  </link>
  
 <link name="lwheel">
    <visual>
      <geometry>
        <cylinder length="0.04" radius="0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
  </link>

  <joint name="base_to_lwheel" type="fixed">
    <parent link="base_link"/>
    <child link="lwheel"/>
    <origin xyz="-0.1 -0.2 -0.025" rpy="1.5708 0 0"/>
    <axis xyz="-0.1 -0.2 -0.025 " />
  </joint>

 <link name="rwheel">
    <visual>
      <geometry>
        <cylinder length="0.04" radius="0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
  </link>
  
  <joint name="base_to_rwheel" type="fixed">
    <parent link="base_link"/>
    <child link="rwheel"/>
    <origin xyz="-0.1 0.2 -0.025" rpy="-1.5708 0 0"/>
  </joint>
  
  <link name="fwheel_left">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.03"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
  </link>
  
  <joint name="base_to_fwheel_left" type="fixed">
    <parent link="base_link"/>
    <child link="fwheel_left"/>
    <origin xyz="0.22 -0.1 -0.095" rpy="1.5708 0 0"/>
  </joint>

  <link name="fwheel_right">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.03"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
  </link>
  
  <joint name="base_to_fwheel_right" type="fixed">
    <parent link="base_link"/>
    <child link="fwheel_right"/>
    <origin xyz="0.22 0.1 -0.095" rpy="-1.5708 0 0"/>
  </joint>

   <link name="scanner">
    <visual>
      <geometry>
        <box size="0.28 0.065 0.04"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
  </link>

   <link name="scan_support">
    <visual>
      <geometry>
        <cylinder length="0.4" radius="0.015"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="black">
        <color rgba="1 0.2 0.1 1"/>
      </material>
    </visual>
  </link>
  
  <joint name="base_to_scan_support" type="fixed">
    <parent link="base_link"/>
    <child link="scan_support"/>
    <origin xyz="0.15 0 0.25" rpy="0 0 0"/>
 </joint>

  <joint name="base_to_scanner" type="fixed">
    <parent link="scan_support"/>
    <child link="scanner"/>
    <origin xyz="0.045 0 0.18" rpy="0 0 -1.5708"/>
 </joint>

The odom publisher is directly taken from the tutorials, from the link as given here: //wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

and using the launch file with code as below:

<launch>
        <param name="robot_description" command="cat $(find my_robot_urdf)/my_robot.urdf" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
        <node name="my_state_publisher" pkg="security_robot_urdf" type="my_state_publisher" />
</launch>

Then I open the Rviz and cjoose odom as my fixed frame and add display of "Robot Model", then simulation shows the robot drifitng as explained above.

Can any one know what I am missing in this.

Many thanks in advance.

---

Originally posted by sumanth on ROS Answers with karma: 86 on 2014-08-01

Post score: 0

---

Original comments

Comment by Martin Peris on 2014-08-01:
Is your robot holonomic or non-holonomic? For a holonomic robot, what you describe seems to be the correct behavior

Comment by sumanth on 2014-08-01:
Frankly speaking I don't know the difference between holonomic and non-holonomic mine is a differential drive.

Comment by Martin Peris on 2014-08-01:
Here is a short explanation about the holonomic robots http://en.wikipedia.org/wiki/Holonomic_(robotics) Anyway, if your robot is differential drive and doesn't have omnidirectional wheels then it is non-holonomic and it should not be able to move as you describe.

Comment by Martin Peris on 2014-08-01:
could you post more info? Like the URDF file, which nodes are you exactly running, any source code that could give us some more clues?

Comment by sumanth on 2014-08-01:
I have edited my question with the required information, can you please refer back to the question and guide me if you get some insights into the problem

Comment by sumanth on 2014-08-01:
Martin, But if I use the example code in the link: //wiki.ros.org/urdf/Tutorials/Using urdf with robot_state_publisher I am getting the perfect result as desired but what's the difference between both.

Answer 1

The problem is in the way you calculate the odometry. You mention that you took the code from the example here. But that code is for a Holonomic robot (it can move independently on the Y direction), but your robot is non-holonomic.

Based on the code mentioned before, here is how you should calculate the odometry of your robot:

//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) /* - vy * sin(th)  */ ) * dt; //for non-holonomic robots vy*sin(th)=0
double delta_y = (vx * sin(th) /* + vy * cos(th)  */ ) * dt; //for non-holonomic robots vy*cos(th)=0
double delta_th = vth * dt;

x += delta_x;
y += delta_y;
th += delta_th;

//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

//send the transform
odom_broadcaster.sendTransform(odom_trans);

//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";

//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;

//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = 0; // for non-holonomic robots this is always 0
odom.twist.twist.angular.z = vth;

//publish the message
odom_pub.publish(odom);

I hope this solves your issue

---

Originally posted by Martin Peris with karma: 5625 on 2014-08-04

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by sumanth on 2014-08-04:
super fine martin :) Many Thanks martin,

Comment by Martin Peris on 2014-08-04:
Glad I could help. If this solved your question, please mark the answer as "accepted", thanks

Comment by sumanth on 2014-08-04:
Martin I want to build on this,

I have ticks coming form the motor encoder's of the robot, How can I use this these ticks to calculate the position of robot in RVIZ, so that the actual robot and the robot in RVIZ are doing the same action or in sync.

Comment by sumanth on 2014-08-04:
let Dc, Dr (distance by right wheel), Dl (by left wheel), L (wheel Base) Dc = Dr+Dl/2

then delta_x = Dccos(th), delta_y = Dcsin(th), delta_th = (Dr - Dl )/L.

Will this be fine (thinking robot as differential drive).

Comment by Martin Peris on 2014-08-04:
Yes, that is pretty much it :) For future reference, it is usually better to post this kind of follow up questions as new questions with a reference to the original question, this way the forum stays organized and more people can benefit from it.

Comment by sumanth on 2014-08-04:
Yeah definitely, Thanks for the support.