euler-angle-quaternion conversion
for euler angle and quaternion:
kdl conversion better than “yuiwongcppbase angle.hpp” conversion (maybe / not)
both ok. and sometimes maybe eigen conversion cannot work??
see “yuiwongtransformdemo” test and “yuiwongcppbase angle test”
#
by kdl
# ros package: `tf_conversions` when ros
# cmake
find_package(orocos_kdl REQUIRED)
find_package(kdl_conversions REQUIRED)
${orocos_kdl_LIBRARIES}
${kdl_conversions_LIBRARIES}
euler angle r. p. y. to quaternion
Quaternion Quaternion::fromRpy(
double const roll, double const pitch, double const yaw)
{
tf::Quaternion tfq;
tfq.setRPY(roll, pitch, yaw);
return Quaternion::fromTfQuaternion(tfq);
}
quaternion to r. p. y.
std::tuple<double, double, double> Quaternion::getRpy() const
{
KDL::Rotation k;
tf::quaternionMsgToKDL(*this, k);
double roll, pitch, yaw;
k.GetRPY(roll, pitch, yaw);
return std::tuple<double, double, double>(roll, pitch, yaw);
}
conversion
euler angle r. p. y. to quaternion
Eigen::Quaterniond RpyToQuaternion(
double const roll, double const pitch, double const yaw)
{
/* Abbreviations for the various angular functions */
double const cy = ::cos(yaw * 0.5);
double const sy = ::sin(yaw * 0.5);
double const cr = ::cos(roll * 0.5);
double const sr = ::sin(roll * 0.5);
double const cp = ::cos(pitch * 0.5);
double const sp = ::sin(pitch * 0.5);
Eigen::Quaterniond q;
q.w() = cy * cr * cp + sy * sr * sp;
q.x() = cy * sr * cp - sy * cr * sp;
q.y() = cy * cr * sp + sy * sr * cp;
q.z() = sy * cr * cp - cy * sr * sp;
return q;
}
quaternion to r. p. y.
std::tuple<double, double, double> QuaternionToRpy(
Eigen::Quaterniond const& q)
{
/* roll (x-axis rotation) */
double const sinr = +2.0 * (q.w() * q.x() + q.y() * q.z());
double const cosr = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
double const roll = ::atan2(sinr, cosr);
/* pitch (y-axis rotation) */
double const sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
double pitch;
if (DoubleCompare(::fabs(sinp), 1.0) > 0) {
/* use 90 degrees if out of range */
pitch = ::copysign(M_PI / 2.0, sinp);
} else {
pitch = ::asin(sinp);
}
/* yaw (z-axis rotation) */
double const siny = +2.0 * (q.w() * q.z() + q.x() * q.y());
double const cosy = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
double const yaw = ::atan2(siny, cosy);
return std::tuple<double, double, double>(roll, pitch, yaw);
}
by eigen
see
https://yuiwong.org/gitlab/cpp/cppcomm/blob/master/yuiwongcppbase/include/yuiwong/angle.hpp
