Ask Your Question
4

Quaternion to Roll, Pitch, Yaw

asked 2013-03-20 10:55:17 -0500

nemesis gravatar image

updated 2014-01-28 17:15:49 -0500

ngrennan gravatar image

Hey, I am attempting to modify a source code (part of it given below) It currently just publishes the Quaternion but I also wish to display RPY. I referred to Rotation Methods and tried the getRPY method. But that doesn't work for me. I believe, since I am new to this (the C++ code, I have a very basic understanding) so I am not sure how to modify the code and publish the RPY values. Can anyone guide me or if this has been previously attempted before then I would appreciate it if someone could post the link. Thank you!

Code - Get values

void VRPN_CALLBACK track_target (void *, const vrpn_TRACKERCB t)
{
btQuaternion q_orig(t.quat[0], t.quat[1], t.quat[2], t.quat[3]);
btQuaternion q_fix(0.70710678, 0., 0., 0.70710678);
btQuaternion q_rot = q_fix * q_orig * q_fix.inverse();

btVector3 axis = q_rot.getAxis();
btVector3 pos(t.pos[0], -t.pos[2], t.pos[1]);

// verifying that each callback indeed gives fresh data.
if ( prev_vrpn_data.quat[0] == t.quat[0] and \
     prev_vrpn_data.quat[1] == t.quat[1] and \
     prev_vrpn_data.quat[2] == t.quat[2] and \
     prev_vrpn_data.quat[3] == t.quat[3] and \
     prev_vrpn_data.pos[0] == t.pos[0] and \
     prev_vrpn_data.pos[1] == t.pos[1] and \
     prev_vrpn_data.pos[2] == t.pos[2] )
    ROS_WARN("Repeated Values");

prev_vrpn_data = t;

target_state->target.transform.translation.x = pos.x();
target_state->target.transform.translation.y = pos.z();
target_state->target.transform.translation.z = -pos.y();

target_state->target.transform.rotation.x = q_rot.x();
target_state->target.transform.rotation.y = q_rot.y();
target_state->target.transform.rotation.z = q_rot.z();
target_state->target.transform.rotation.w = q_rot.w();

target_state->target.header.frame_id = "optitrak";
target_state->target.child_frame_id = frame_id;
target_state->target.header.stamp = ros::Time::now();

fresh_data = true;
}

Code which should Publish the values

int main(int argc, char* argv[])
{
ros::init(argc, argv, "vrpn_tracked_object_1");
ros::NodeHandle nh("~");
target_state = new TargetState;
frame_id = nh.getNamespace();
std::string vrpn_server_ip;
int vrpn_port;
std::string tracked_object_name;
nh.param<std::string>("vrpn_server_ip", vrpn_server_ip, std::string());
nh.param<int>("vrpn_port", vrpn_port, 3883);

std::cout<<"vrpn_server_ip:"<<vrpn_server_ip<<std::endl;
std::cout<<"vrpn_port:"<<vrpn_port<<std::endl;

Rigid_Body tool(nh, vrpn_server_ip, vrpn_port);

ros::Rate loop_rate(1000);

while(ros::ok())
{
    tool.step_vrpn();
    if (fresh_data == true)
    { // only publish when receive data over VRPN.
        tool.publish_target_state(target_state);
        fresh_data = false;
    }
    loop_rate.sleep();
}
return 0;
}

I hope posting this much code is not an issue. If anyone could give an insight/explanation into how to obtain and publish the RPY values, I would appreciate it. (Plus it would also help me understand the code much better) Thank you!

Edit: My incorrect solution

//The following was added to int main()

btQuaternion q(target_state->target.transform.rotation.x, target_state->target.transform.rotation.y, target_state->target.transform.rotation.z, target_state->target.transform.rotation.w);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

//The following was added to the while(ros::ok()) loop inside int main()

std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;

The above does give me certain values, but they are incorrect It gives me ... (more)

edit retag flag offensive close merge delete

Comments

In general, it helps when questions contain the minimal amount of code that reproduces issues related to the question. I would encourage you to post that type minimal code examples with your questions in the future, such as you did here.

Thomas D gravatar imageThomas D ( 2013-03-20 11:00:42 -0500 )edit

@nemesis, do you know how should i do it if i am having a set of quaternion angles and i would like to apply rpy to it? e.g. orientation.x = quat[0]; orientation.y = quat[1]; orientation.z = quat[2]; orientation.w = quat[3]; and i would like to add 180deg to pitch. thanks.

chao gravatar imagechao ( 2014-01-02 19:53:49 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
4

answered 2013-03-21 11:47:15 -0500

nemesis gravatar image

updated 2013-03-21 14:04:34 -0500

I had to post this as an answer -

As per suggestion in this post. I modified the source code of the package (in reference to sample code in my post above) - Modified the following -

btQuaternion q_orig(t.quat[0], t.quat[1], t.quat[2], t.quat[3]);

to -

btQuaternion q_orig(t.quat[0], t.quat[1], -t.quat[2], t.quat[3]);

Also, I (as already mentioned) added the following to the code to get the RPY values.

Note Unlike what I mentioned in my Question above, the following code is added to if (fresh_data == true) loop inside int main() -

btQuaternion q(target_state->target.transform.rotation.x, target_state->target.transform.rotation.y, target_state->target.transform.rotation.z, target_state->target.transform.rotation.w);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

This is giving me the correct values as per the Optitrack Tracking Tools display.

Thanks everyone for their help!

edit flag offensive delete link more
4

answered 2013-03-20 12:12:50 -0500

georgwi gravatar image

updated 2013-03-20 12:26:19 -0500

Could you be more specific on what does not work? Since you say getRPY does not work for you I take it you get a compile error? Or does the function simply give false values? I see you got some btQuaternions there. First you have to turn this quaternion in a rotation matrix and then use the accessor getRPY on this matrix. Lets assume btQuaternion quat is the quaternion you want to get roll pitch and yaw from:

double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

Now all you have to do is write this information in a message of your choice and publish it. For some reason the btMatrix3x3 does not seem to have the desired member function, so I use tf::Matrix3x3. Make sure you included all the needed stuff:

#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

For the Publishing: If you want to publish inside the VRPN_CALLBACK function you have to create your publisher globally. This is not the best method, but it will probably work fine for you:

geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);

and globally:

#include "geometry_msgs/Vector3.h"
ros::Publisher rpy_publisher;

and below your node handle:

rpy_publisher = nh.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);

hope I did not forget something :)

edit: Oh you got it but there are incorrect values. Sorry, but I think I can not help you there. Maybe you could check if the error is in some way related to pi/2 or the values are exchanged. Then you could adapt your coordinate system...

edit2: Did you just initialize q in the main() routine? If yes that might be your problem. You have to write some values in q otherwise you will get only random numbers as output :)

edit flag offensive delete link more

Comments

@georgwi - I tried the code which you had posted in that other post, but it gave the error mentioning that "rpy wasn't declared" and another similar error.I thought it might vary depending on the package code. I attempted to cross-check the degrees/radians issue. Value still doesn't match.

nemesis gravatar imagenemesis ( 2013-03-20 12:31:01 -0500 )edit

@georgwi - I actually did try that too. I once tried putting it as q(target_state->target.transform.rotation.x, //same for y and z). I got output as all values zeros. Maybe I am referring to the values of the quaternion incorrectly?

nemesis gravatar imagenemesis ( 2013-03-20 12:36:19 -0500 )edit
1

you must also copy the value of w from the target state. The Quaternion consists of 4 values. I probably would put all the conversion and publishing inside the if (fresh_data == true) loop. Or you could publish in the callback function and use q_rot directly

georgwi gravatar imagegeorgwi ( 2013-03-20 20:40:31 -0500 )edit

@georgwi - Thanks! It does return me values which are almost similar to the actual values that I have. I created a new post to ask about the getRPY() to understand this better. I also edited my post above to include my attempted solution

nemesis gravatar imagenemesis ( 2013-03-21 07:12:44 -0500 )edit
3

answered 2013-03-20 10:59:26 -0500

Thomas D gravatar image

There is a similar question with answers containing sample code for Python and C++.

edit flag offensive delete link more

Comments

@Thomas D - Thank you! Although that code seems to be a lot different as compared to the source code of the package (as given in my main post above), so I can't directly just use that I guess.

nemesis gravatar imagenemesis ( 2013-03-20 11:31:32 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-03-20 10:55:17 -0500

Seen: 42,416 times

Last updated: Mar 21 '13