Robotics StackExchange | Archived questions

How to know the direction that the Robot is pointing to from pose?

I am making a plot of the position and orientation of the robot in excel, I want to represent the direction of the robot in free space. But, I have a quaternion value from robot's pose like below:

pose = [0.23853423125670908, -0.00658252361852945, -0.058558705984261855]

How do I use this quaternion = -0.058558705984261855 and represent the direction of the robot?

Please if somebody has a clue. Its very urgent.

Thanks

Asked by ish45 on 2014-11-07 00:43:13 UTC

Comments

You should provide more information. Where do you get these values from? What robot are you talking about (something like turtlebot with x,y,theta or a arm with an end effector)? What is the root of you world coordinate system?

Asked by Rabe on 2014-11-07 07:39:39 UTC

It is a mobile robot like turtlebot. The message type is Odometry obtained from nav_msgs.msg. I obtain pose from this message type. In this pose, the first two values are easting and northing. The third value is the yaw value. Can you tell me if its possible to know direction using yaw value?

Asked by ish45 on 2014-11-07 07:55:49 UTC

In that case, as dornhege said, the direction IS the yaw value. At some point you have defined a world reference system. Maybe it is the first position the robot is in, when you start the program. From this orientation, the yaw value tells you in which direction the robot is currently pointing.

Asked by Rabe on 2014-11-07 08:57:29 UTC

Thanks. But how do I use yaw = -0.058558705984261855 and give it a direction in the form of an arrow? Seems difficult to me.

Asked by ish45 on 2014-11-07 10:31:28 UTC

I don't know how to plot in excel. Depending on how you define an arrow in excel, you might need to transform the value from rad to degree. But I guess for plotting in excel, this is the wrong forum to ask.

Asked by Rabe on 2014-11-07 11:39:19 UTC

So, do you agree that the yaw value that I have mentioned is in radians?

Asked by ish45 on 2014-11-07 12:11:52 UTC

It most likely is. But you can check that very easily via moving the robot. Let it turn for 180degrees and check the values. Or keep the orientation and just move forward a bit, check which values change by how much.

Asked by Rabe on 2014-11-07 12:18:30 UTC

If you know how to draw a line segment in Excel (is that possible?) and you just want to see which direction it's pointing, make the line start point (0, 0) and the end (cos(yaw), sin(yaw)).

Asked by Tom Moore on 2014-11-07 13:34:19 UTC

Answers

I think you might have mixed up some words (and since english isn't my native language, the following might not be 100% correct):

A quaternion always has 4 values. Since your "pose" has 3, it either is the position in xyz and you dereferenced the values wrong. Or it is the orientation in euler values, most likely in r(oll) p(itch) y(aw), which basicly is the value you are looking for in radians.

As I said, at this point I am not to certain if "pose" is either another word for "orientation" or for "position". But those values are definetly not a quaternion.

Asked by Rabe on 2014-11-07 05:24:11 UTC

Comments

Yet another guess might be that it's x, y, theta.

Asked by dornhege on 2014-11-07 06:46:10 UTC

You are right. Quaternion are 4 values. Do you know how to know the direction of the robot or the heading from the these 4 quaternion values?

Asked by ish45 on 2014-11-07 06:47:26 UTC

You are right that value is the yaw value. Do you know how to convert this yaw value and know the direction that the robot is pointing towards?

Asked by ish45 on 2014-11-07 06:58:53 UTC

If it is a yaw, the robot is pointing in the direction of the yaw value.

Asked by dornhege on 2014-11-07 07:41:43 UTC

Here's a sample nav_msgs/Odometry message, the first part of which is a geometry_msgs/Pose message:

header: 
  seq:        239
  stamp: 
    secs: 1408542253
    nsecs:  730130796
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 19.4294389
      y: -1.4772684
      z: -0.6923008
    orientation: 
      x: 0.01773404
      y: 0.01089619
      z: -0.0490339
      w: 0.99858021
  covariance: [0.00145422, 0.00000001, 0.00000018, -0.0000000, 0.00000000, 0.00000000, 0.00000001, 0.00145451, -0.0000003, -0.0000000, -0.0000000, 0.00000000, 0.00000018, -0.0000003, 0.01903145, 0.00000000, -0.0000000, 0.00000000, -0.0000000, -0.0000000, 0.00000000, 0.00000099, 0.00000000, 0.00000000, 0.00000000, -0.0000000, -0.0000000, 0.00000000, 0.00000099, -0.0000000, 0.00000000, 0.00000000, 0.00000000, 0.00000000, -0.0000000, 0.00000099]

The part you need is the "orientation." It is represented as a quaternion instead of Euler angles. To get the Euler angles, you need to carry out a conversion. If you want to do it with C++, you can do something like this:

tf::Quaterion quat;
tf::quaternionMsgToTF(msg->pose.pose.orientation, orientation);
tf::Matrix3x3 orTmp(quat);
orTmp.getRPY(roll, pitch, yaw);

If you're doing it in Excel, you'll need to carry out the conversion yourself. If you're simply after the robot's heading, you want the "yaw" value.

Asked by Tom Moore on 2014-11-07 09:18:33 UTC

Comments

Thanks for your answer. Now, I got this yaw value = -0.058558705984261855. How do I create an arrow in the direction pointed by this yaw value. Which direction does this point to. Assuming initially, the robot had yaw = 0. I have never attempted anything like this before.

Asked by ish45 on 2014-11-07 10:33:18 UTC

Sorry, just making sure I understand you: did you fill out your Odometry message so that the heading (yaw) is stored in the Z position variable? Your "pose" should have (X, Y, Z) position. The orientation is defined using the (X, Y, Z, W) quaternion.

Asked by Tom Moore on 2014-11-07 13:29:22 UTC