Robotics StackExchange | Archived questions

Turn a quaternion

Hey there I have a quaternion q and I would like to turn that.

Therefore I tried:

   tf::Quaternion q( four_result_pose.orientation.x,  four_result_pose.orientation.y,  four_result_pose.orientation.z,  four_result_pose.orientation.w);
   tf::Matrix3x3 m = tf::getRotation(q);
   tf::Matrix3x3 x_90_deg_turn;
   tf::Matrix3x3 result_matrix = m *x_90_deg_turn.setRPY(1,0,0);  //(TODO - DOES NOT WORK)

 // TODO how to give back the quaternion result?

Is there any Matrix3x3 function to convert a Matrix3x3 to a quaternion???

Solution

   tf::Quaternion q( four_result_pose.orientation.x,  four_result_pose.orientation.y,  four_result_pose.orientation.z,  four_result_pose.orientation.w);
   tf::Matrix3x3 m(q);
   tf::Matrix3x3 x_90_deg_turn;
   x_90_deg_turn.setEulerYPR(+1.57079632679, 0, 0);
   tf::Matrix3x3 result_matrix = m*x_90_deg_turn;

   double res_yaw;
   double res_pitch;
   double res_roll;
   result_matrix.getRPY(res_yaw, res_pitch, res_roll);

   q.setRPY(res_yaw, res_pitch, res_roll);

Do not know how to turn it correctly??

How to set setEulerYPR(+1.57079632679, 0, 0); So that the outer coordinate systems are turned into the one in the middle around the x axis by 90 degree. so that the y axis goes up the x axis right and the z axis out of the image.

See this image:

C:\fakepath\Screenshot from 2018-05-30 11:20:49.png

Asked by Markus on 2018-05-30 03:31:02 UTC

Comments

wiki/tf2/Tutorials/Quaternions might help you here.

Asked by gvdhoorn on 2018-05-30 03:42:14 UTC

.setEulerYPR(+1.57079632679, 0, 0); rotates +pi/2 around z-axis, not x. See here for the definition

And the matrix multiplication should be x_90_deg_turn*m instead of m*x_90_deg_turn

Asked by vinaykumarhs2020 on 2018-05-30 13:12:40 UTC

Answers

You need to split the setRPY call and the multiplication into two different statements. At the moment the last line of your code is trying to multiply m a tf::Matrix3x3 by the returned value of the setRPY() method. This is a void function so you code is trying to do tf::Matrix3x3 * void hence your problem. Try the following code instead:

tf::Quaternion q( four_result_pose.orientation.x,  four_result_pose.orientation.y,  four_result_pose.orientation.z, four_result_pose.orientation.w);
tf::Matrix3x3 m = tf::getRotation(q);
tf::Matrix3x3 x_90_deg_turn;
x_90_deg_turn.setRPY(1,0,0);
tf::Matrix3x3 result_matrix = m *x_90_deg_turn;

This should solve your problem.

Asked by PeteBlackerThe3rd on 2018-05-30 03:41:28 UTC

Comments