OrientationConversion.cpp

Go to the documentation of this file.

 
/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXGuiPlugins::RobotTrajectoryDesigner::Interpolation
 * @author     Timo Birr
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "OrientationConversion.h"
 
#include "RobotAPI/libraries/core/FramedPose.h"
 
 
using namespace armarx;
 
namespace armarx
{
    std::vector<double>
    OrientationConversion::toEulerAngle(QuaternionBasePtr q)
    {
        // roll (x-axis rotation)
        double sinr = +2.0 * (q->qw * q->qx + q->qy * q->qz);
        double cosr = +1.0 - 2.0 * (q->qx * q->qx + q->qy * q->qy);
        double roll = atan2(sinr, cosr);
        // pitch (y-axis rotation)
        double sinp = +2.0 * (q->qw * q->qy - q->qz * q->qx);
        double pitch;
        if (fabs(sinp) >= 1)
        {
            pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
        }
        else
        {
            pitch = asin(sinp);
        }
        // yaw (z-axis rotation)
        double siny = +2.0 * (q->qw * q->qz + q->qx * q->qy);
        double cosy = +1.0 - 2.0 * (q->qy * q->qy + q->qz * q->qz);
        double yaw = atan2(siny, cosy);
        return {roll, pitch, yaw};
    }
 
    QuaternionBasePtr
    OrientationConversion::toQuaternion(const double roll, const double pitch, const double yaw)
    {
        QuaternionBasePtr q = QuaternionBasePtr(new FramedOrientation());
        //Abbreviations for the various angular functions
        double cy = cos(yaw * 0.5);
        double sy = sin(yaw * 0.5);
        double cr = cos(roll * 0.5);
        double sr = sin(roll * 0.5);
        double cp = cos(pitch * 0.5);
        double sp = sin(pitch * 0.5);
        q->qw = cy * cr * cp + sy * sr * sp;
        q->qx = cy * sr * cp - sy * cr * sp;
        q->qy = cy * cr * sp + sy * sr * cp;
        q->qz = sy * cr * cp - cy * sr * sp;
        return q;
    }
 
    Eigen::Quaternion<double>
    OrientationConversion::toEigen(QuaternionBasePtr q)
    {
        return Eigen::Quaternion<double>(q->qw, q->qx, q->qy, q->qz);
    }
 
    QuaternionBasePtr
    OrientationConversion::toArmarX(Eigen::Quaternion<double> q)
    {
        FramedOrientation temp = *new FramedOrientation();
        temp.qw = q.w();
        temp.qx = q.x();
        temp.qy = q.y();
        temp.qz = q.z();
        return QuaternionBasePtr(new FramedOrientation(temp));
    }
} // namespace armarx