euler to quaternion c++
Hi, because I'm quite new to world of ROS, I'm struggling with transforming this Python line to c++ -
quaternion = quaternion_from_euler(roll, pitch, yaw)
I have tried multiple approaches. I used :
setEuler(), setRPY and other,but when I conver the RPY to quaternion and compare the results in online calculators it doesn't match. Could someone give me a clue how to solve this issue?
Asked by amaTMR on 2021-02-16 07:35:46 UTC
Answers
I'll just post my code i am using for this since there isn't much to explain about it:
tf2::Quaternion myQuaternion;
myQuaternion.setRPY(r,p,y);
myQuaternion=myQuaternion.normalize();
and you are done.
you then get the quaternion parts with e.g myQuaternion.getX() ...
The normalize part is only in there since i had issues in the past with not normalized quaternions being rejected by some libraries so i just normalize everything. But this can be the reason, why your result looked different than the one from the online calculator. Hope this helps
EDIT: you need the following libraries: tf2/LinearMath/Quaternion.h
Asked by Tristan9497 on 2021-02-16 12:10:08 UTC
Comments
Thank you,I used your code,but I'm oddly getting different results. This is what I've got :
Asked by amaTMR on 2021-02-17 03:37:50 UTC
tf2::Quaternion q; q.setRPY(0,0,0); q= q.normalize();
ROS_INFO_STREAM("\n qUART unchanged **********" );
ROS_INFO_STREAM("qUART x " << q[0] );
ROS_INFO_STREAM("qUART y" <<q[1]);
ROS_INFO_STREAM("qUART z " <<q[2]);
ROS_INFO_STREAM("qUART w" <<q[3]);
ROS_INFO_STREAM("qUART transformed ********* " );
q.setRPY(20.000,-60.000, 30.000);
q= q.normalize();
ROS_INFO_STREAM("qUART x " << q[0] );
ROS_INFO_STREAM("qUART y" <<q[1]);
ROS_INFO_STREAM("qUART z " <<q[2]);
ROS_INFO_STREAM("qUART w" <<q[3]);
ROS_INFO_STREAM("qUART transformed ********* \n" );
Asked by amaTMR on 2021-02-17 03:40:05 UTC
[ INFO] [1613550291.060752163]: qUART x 0 [ INFO] [1613550291.060818770]: qUART y0 [ INFO] [1613550291.060867797]: qUART z 0 [ INFO] [1613550291.060917555]: qUART w1 [ INFO] [1613550291.060961065]: qUART transformed [ INFO] [1613550291.061027826]: qUART x 0.602858 [ INFO] [1613550291.061087748]: qUART y0.575234 [ INFO] [1613550291.061162861]: qUART z -0.492505 [ INFO] [1613550291.061209418]: qUART w-0.251211
Asked by amaTMR on 2021-02-17 03:41:47 UTC
Unfortunately i cant see the result of your conversion.
But it seems like you want to set rpy in degree.
setRPY() function expects radian (Hint basically everywhere in ros where u use euler angles these are radian).
Try multiplying your angles by pi/180 to convert them and check again.
I'd maybe even write a small function for the conversion, not sure if thats your style though.
In the end it should look something like this:
q.setRPY(r*M_PI/180,p*M_PI/180,y*M_PI/180);
q=q.normalize();
Asked by Tristan9497 on 2021-02-17 09:35:09 UTC
Thanks,I solved this issue by using this :
quaternion_.setRPY(roll,pitch,yaw);
quaternion_ = quaternion_.normalize();
transformStamped_.transform.rotation.x = quaternion_.x();
transformStamped_.transform.rotation.y = quaternion_.y();
transformStamped_.transform.rotation.z = quaternion_.z();
transformStamped_.transform.rotation.w = quaternion_.w();
My mistake was that I compared my data to the online calculator.which differs and I have no clue why. But my node works now ,thanks again...
Asked by amaTMR on 2021-02-19 06:51:01 UTC
You're welcome happy i was able to help.
Asked by Tristan9497 on 2021-02-19 07:34:30 UTC
Comments