Ask Your Question
0

Robot Drifting in rviz

asked 2014-08-01 03:37:32 -0500

sumanth gravatar image

updated 2014-08-01 06:19:00 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Is your robot holonomic or non-holonomic? For a holonomic robot, what you describe seems to be the correct behavior

Martin Peris gravatar imageMartin Peris ( 2014-08-01 03:52:30 -0500 )edit

Frankly speaking I don't know the difference between holonomic and non-holonomic mine is a differential drive.

sumanth gravatar imagesumanth ( 2014-08-01 04:47:37 -0500 )edit

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.

Martin Peris gravatar imageMartin Peris ( 2014-08-01 05:42:18 -0500 )edit

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?

Martin Peris gravatar imageMartin Peris ( 2014-08-01 05:43:20 -0500 )edit

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

sumanth gravatar imagesumanth ( 2014-08-01 06:20:06 -0500 )edit

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.

sumanth gravatar imagesumanth ( 2014-08-01 07:31:42 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2014-08-04 01:38:00 -0500

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

edit flag offensive delete link more

Comments

super fine martin :) Many Thanks martin,

sumanth gravatar imagesumanth ( 2014-08-04 03:09:42 -0500 )edit

Glad I could help. If this solved your question, please mark the answer as "accepted", thanks

Martin Peris gravatar imageMartin Peris ( 2014-08-04 03:28:10 -0500 )edit

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.

sumanth gravatar imagesumanth ( 2014-08-04 04:11:12 -0500 )edit

let Dc, Dr (distance by right wheel), Dl (by left wheel), L (wheel Base) Dc = Dr+Dl/2 then delta_x = Dc*cos(th), delta_y = Dc*sin(th), delta_th = (Dr - Dl )/L. Will this be fine (thinking robot as differential drive).

sumanth gravatar imagesumanth ( 2014-08-04 04:14:01 -0500 )edit

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.

Martin Peris gravatar imageMartin Peris ( 2014-08-04 19:52:03 -0500 )edit

Yeah definitely, Thanks for the support.

sumanth gravatar imagesumanth ( 2014-08-04 23:58:49 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-08-01 03:37:32 -0500

Seen: 882 times

Last updated: Aug 04 '14