[ros-users] tf and quaternions

[Wim Meeussen]

> Anyway, all I wanted to do was to get the Z rotation value of one observed
> frame. If feels strange to have gone all the way with using KDL for that.
> Here is the code (not optimized) I came up with. Do you see a more efficient
> way to do this?
>
>    //compute roll pitch yaw using KDL
>    tf::Vector3 axis=quat.getAxis();
>    float anglekd = quat.getAngle();
>    KDL::Vector vectkd = KDL::Vector(axis.x(), axis.y(), axis.z());
>    KDL::Rotation rot = KDL::Rotation();
>    rot = rot.Rot(vectkd, anglekd);
>    double rx, ry, rz;
>    rot.GetEulerZYX(rz, ry, rx);

or you could write:

double rx, ry, rz;
KDL::Rotation.Quaternion(quat.x, quat.y, quat.z,
quat.w).GetEulerZYX(rz, ry, rx);


Wim

-- 
--
Wim Meeussen
Willow Garage Inc.
<http://www.willowgarage.com)