euler-angle-quaternion

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

see also