OrientationConversionTest.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::OrientationConversionTest
#define ARMARX_BOOST_TEST
 
#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
 
#include "RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h"
#include "RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.h"
#include "RobotAPI/libraries/core/Pose.h"
 
#include "RobotAPI/libraries/core/FramedPose.h"
using namespace armarx;
using namespace boost::test_tools;
BOOST_AUTO_TEST_CASE(eulerToQuatTest1)
{
    //Init
    QuaternionBasePtr ori = OrientationConversion::toQuaternion(1, 0, 0);
    BOOST_REQUIRE_CLOSE(ori->qw, 0.878, 1);
    BOOST_REQUIRE_CLOSE(ori->qx, 0.479, 1);
    BOOST_REQUIRE_CLOSE(ori->qy, -0.0, 1);
    BOOST_REQUIRE_CLOSE(ori->qz, 0, 1);
}
 
BOOST_AUTO_TEST_CASE(eulerToQuatTest2)
{
    //Init
    QuaternionBasePtr ori = OrientationConversion::toQuaternion(3, 1, -2);
    BOOST_REQUIRE_CLOSE_FRACTION(ori->qw,  -0.369, 1);
    BOOST_REQUIRE_CLOSE(ori->qx, 0.502, 1);
    BOOST_REQUIRE_CLOSE(ori->qy, -0.718, 1);
    BOOST_REQUIRE_CLOSE(ori->qz, -0.311, 1);
}
BOOST_AUTO_TEST_CASE(eulerToQuatTest3)
{
    //Init
    QuaternionBasePtr ori = OrientationConversion::toQuaternion(1, -1, 2);
    BOOST_REQUIRE_CLOSE(ori->qw, 0.223, 1);
    BOOST_REQUIRE_CLOSE(ori->qx, 0.581, 1);
    BOOST_REQUIRE_CLOSE(ori->qy, 0.127, 1);
    BOOST_REQUIRE_CLOSE(ori->qz, 0.772, 1);
}
 
 
 
 
BOOST_AUTO_TEST_CASE(quatToEulerTest1)
{
    //Init
    QuaternionBasePtr ori = *new QuaternionBasePtr(new FramedOrientation());
    ori->qw = 0.5;
    ori->qx = 0.5;
    ori->qy = 0.5;
    ori->qz = 0.5;
    std::vector<double> euler = OrientationConversion::toEulerAngle(ori);
    BOOST_REQUIRE_CLOSE(euler[0], 1.571, 1);
    BOOST_REQUIRE_CLOSE(euler[1], 0, 1);
    BOOST_REQUIRE_CLOSE(euler[2], 1.571, 1);
}
 
BOOST_AUTO_TEST_CASE(quatToEulerTest2)
{
    //Init
    QuaternionBasePtr ori = *new QuaternionBasePtr(new FramedOrientation());
    ori->qw = 0.658;
    ori->qx =  0.164;
    ori->qy = -0.329;
    ori->qz = 0.658;
    std::vector<double> euler = OrientationConversion::toEulerAngle(ori);
    BOOST_REQUIRE_CLOSE(euler[0], -0.288, 1);
    BOOST_REQUIRE_CLOSE(euler[1], -0.706, 1);
    BOOST_REQUIRE_CLOSE(euler[2], 1.678, 1);
}
 
BOOST_AUTO_TEST_CASE(twoConversionsTest)
{
    QuaternionBasePtr ori = OrientationConversion::toQuaternion(0.1, 0.2, 0.3);
    std::vector<double> euler = OrientationConversion::toEulerAngle(ori);
    BOOST_REQUIRE_CLOSE(euler[0], 0.1, 1);
    BOOST_REQUIRE_CLOSE(euler[1], 0.2, 1);
    BOOST_REQUIRE_CLOSE(euler[2], 0.3, 1);
}