# Rotation Angle in Pose Orientation I have a 2D image on which I conduct an algorithm to find its rotation, and I get it in radian. No problem until here.

Now that I want to fill in the pose object with what I collected from the 2D vision, I get stuck at where to insert the rotation. I do know I cannot fill in everything, I don't intend to, either. I only need the position (I have it already) and the rotation.

            obj.pose.position.x = v['pose'] #x
obj.pose.position.y = v['pose'] #y
obj.pose.position.z = v['pose'] #z

# which one below is the one I should use to pass the rotation angle?
obj.pose.orientation.x = v['pose']
obj.pose.orientation.y = v['pose']
obj.pose.orientation.z = v['pose']
obj.pose.orientation.w = v['pose']


Any thoughts?

EDIT: So I came up with the following after the useful insights given by the commentators.

       self.some_srv = rospy.Service("/bla/request_poses", PartArray, self.some_service_srv)

def some_service_srv(self, req):
# some irrelevant stuff happening here

# this is where the actual thing shall happen
obj.pose.position.x = v['pose']
obj.pose.position.y = v['pose']
obj.pose.position.z = v['pose']
rotation_angle = v['pose'] # extract the rotation angle
zaxis = (0, 0, 1) #rotation is around the Z axis in the image
quaternion = quaternion_about_axis(rotation_angle, zaxis) # use rotation angle + axis to get the quaternion
obj.pose.orientation.x = quaternion
obj.pose.orientation.y = quaternion
obj.pose.orientation.z = quaternion # this is the rotation we are talking about
obj.pose.orientation.w = quaternion

list_detected.part_array.append(obj)
return list_detected


This runs, however I always get 0 values for all of the quarternions, which is somehow nonsense because the rotation angle is definitely non-zero (I print its value).

            position:
x: 274.0
y: 250.0
z: 602.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0


I print the quarternion list separately, and there is a non-zero value inside, which seems fine, but I don't know why the published value gets zero.

         [0.000000e+00 0.000000e+00 3.061617e-17 1.000000e+00]


I also printed the value of rotation (in radian) and here are some values of it:

0.980580687945
6.12323399574e-17
-0.707106781187
0.294085862655
-1.0
0.56419054038
-0.0114934175734
6.12323399574e-17
6.12323399574e-17
-1.0
0.707106781187
6.12323399574e-17


And this is how I get the angle using OpenCV:

# helper function to find out which side of the rectangle is longer, then add the angle appropriately
def getAngle(rect):
angle = rect
width, height = rect
if (width < height):
else:

...
# Find contours:
(im, contours, hierarchy) = cv2.findContours(im, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
cnt = contours

# get the rotated rectangle to get the rotation
rect = cv2.minAreaRect(cnt)
rotation = getAngle(rect) # in radians
...

edit retag close merge delete

Considering that the image is on the xy plane, I would say that the rotation is on the orientation.z. Think of it, as turning the image around the z axis, but keeping the other axes stable. If the image is not on one axis, it may be more complicated though

It is indeed more complicated than that, in all cases I'm afraid. See my answer below.

Even if your rotation was zero the quaternion would contain positive values. The quaternion values you printed out is very small (x10-17) so is being rounded to zero. There appears to be a problem with the quaternion_about_axis function since it's returning an invalid rotation quaternion.

Is the function problematic or the way I use it? I don't get it. Is there a bug in ROS, seriously?

The function definitely works, can you show us more of your code. It's probably something about how your getting the values into the message.

I edited my question, butt there isn't much to write really, I mean, I wrote all that is relevant. It's just a service which is called, and it should return the pose (position + quarternion). Position values are filled in, no problem, but the quaternion values are always zero for some reason.

1

Can you print the value and type of v['pose'] is this not numeric then it may explain this behaviour. I've just checked the syntax and I think you're doing anything obvious wrong. The function really should be returning a 4D unit vector

One of the values your printing out 6.12323399574e-17 is Exactly twice the value in the quaternion of 3.061617e-17 I suspect there is some other code you haven't put in your question which is causing the problem. Can you post all your code which is producing the problem.

Sort by » oldest newest most voted The quaternion for a given Z axis (yaw) rotation is given by:

w = cos(theta / 2) x = 0 y = 0 z = sin(theta / 2)

where theta is the yaw angle in radians. If X (roll) or Y (pitch) axis rotations are involved too, the formula is a little more tedious, and can be found on Wikipedia.

The tf library has very helpful functions for constructing quaternions from Euler angles. For yaw quaternions, there is tf::createQuaternionFromYaw(double yaw) which takes a single argument which is the yaw angle in radians and returns a tf::Quaternion object that is equivalent. There is also tf::createQuaternionMsgFromYaw(double yaw) (notice the 'Msg') which is similar, but returns a geometry_msgs::Quaternion ROS message.

For all three axes, there is tf::createQuaternionFromRPY(double roll, double pitch, double yaw) and tf::createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw).

Of course, this is all available in C++ by including <tf/tf.h>. In Python, it looks like you can convert Euler angles into a quaternion using transformations.py (doc_link). I haven't used it myself before though.

more

I am not using C++, but the Python equivalent of what you are saying should be quaternion_about_axis(angle, axis), right? All I have is angle around the axis Z, so this should be it. Edited the question.

Also make sure your quaternions are always normalized otherwise most operations you will do on them will yeld undefined behavior.

Thanks for the warning. Do you have any normalization example I can take a look at?

The quaternion should never be all zeros, as it is always should be a unit-length vector. I would first try manually computing w and z according to the formula in my answer and make sure that you at least get the result that way to rule out any weirdness

It's not all zeros though: [0.000000e+00 0.000000e+00 3.061617e-17 1.000000e+00] does this also look weird?

This looks fine, 3.0e-17 is a very small number, close enough to zero. The norm is this quaternion is 1 so you are good.

Oh, didn't notice that w is 1! Yes, that is a valid quaternion, but it seems like the angle you passed into it is very close to zero. A quaternion with w = 1 and the rest are zero represents no rotation. Rotations in ROS are described using rotation quaternions, which are represented by the four orientation values you see in the pose message. A rotation quaternion must be a 4D unit vector for it to be valid, and it is non-trivial to calculate the elements yourself.

However there are useful helper classes to do this for you. The tf2::Quaternion object has a constructor which accepts angle and axis representation which is exactly what you want in your case. You have a known rotation and you know it's about the Z axis. So if you use this constructor to create a quaternion object you can then use its getX(), getY() . . getW() methods to calculate the values you need.

Hope this makes sense.

more

My rotation is about the Z axis yes, correct. I'll then create that object accordingly and then I'll call those methods to assign obj.pose.orientation.w and so on? Which of these is the rotation though? I still don't know that. Is it Z?