Quaternion rotation

Hi guys, i'm having some problems with the rotation through quaternions. I have two quaternions coming one from a navmsgs::odometry and the other one from a geometrymsgs::pose. I want to rotate the robot until the quaternion of the navmsgs is equal, or almost equal, to the geometrymsgs one. How is it possible to solve my problem? Thanks in advance.

Asked by tonio on 2017-05-24 10:24:42 UTC

Comments

Answers

You can obtain the radians value for the yaw through tf::getYaw and then compute the angular speed command for the robot.

Asked by Chaos on 2017-05-24 10:39:44 UTC

Comments

I obtain this warning: MSG to TF: Quaternion Not Properly Normalized, what's that? By the way, surfing the web, I found some mathematical formulas that state: yaw = atan2(2(wz+xy),1-2(y^2+z^2)). I compared the result from this formula and the result from tf::getYaw and they are different, which use?

Asked by tonio on 2017-05-24 11:04:13 UTC

If you got that error, it means that the quaternion was not properly computed. How do you obtain the two quaternions? The norm of the quaternions should always be 1. In order to compute the yaw, first, you should know how the rotation was built (x-y-z or z-y-x, etc...)

Asked by Chaos on 2017-05-25 02:49:56 UTC

If I set an orientation goal (a geometry_msgs::Pose) equal to (0,0,1,1), it should rotate of 180° but the warning shows up, so what's wrong with this?

Asked by tonio on 2017-05-25 04:30:34 UTC

(0,0,1,1) is not a unitary quaternion. Its norm is 2. You can create quaternion with tf::createQuaternionFromYaw or tf::createQuaternionFromRPY. Check this

Asked by Chaos on 2017-05-25 04:43:19 UTC