> 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)