KinematicSolverTest.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::KinematicSolver
#define ARMARX_BOOST_TEST
 
 
#include "../KinematicSolver.h"
 
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <RobotComponents/Test.h>
 
#include "../Interpolation/InterpolationSegmentFactory.h"
#include "../Interpolation/LinearInterpolation.h"
#include "../Util/OrientationConversion.h"
 
using namespace armarx;
 
using namespace std;
 
using namespace VirtualRobot;
 
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief The PosePkg struct - Testing utility
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
struct PosePkg
{
public:
    Vector3BasePtr pos;
    QuaternionBasePtr ori;
    PoseBasePtr pose;
};
 
PosePkg
createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
{
    PosePkg posePkg;
    posePkg.pos = Vector3BasePtr(new Vector3(x, y, z));
    posePkg.ori = QuaternionBasePtr(new Quaternion(qw, qx, qy, qz));
    PoseBasePtr tmp = PoseBasePtr(new Pose(posePkg.pos, posePkg.ori));
    posePkg.pose = tmp;
    return posePkg;
}
 
bool
isEqualRounded(PoseBasePtr current, PoseBasePtr other, double delta)
{
    if (current->orientation->qw - other->orientation->qw > delta &&
        other->orientation->qw - current->orientation->qw > delta)
    {
        return false;
    }
    if (current->orientation->qx - other->orientation->qx > delta &&
        other->orientation->qx - current->orientation->qx > delta)
    {
        return false;
    }
    if (current->orientation->qy - other->orientation->qy > delta &&
        other->orientation->qy - current->orientation->qy > delta)
    {
        return false;
    }
    if (current->orientation->qz - other->orientation->qz > delta &&
        other->orientation->qz - current->orientation->qz > delta)
    {
        return false;
    }
    if (current->position->x - other->position->x > delta &&
        other->position->x - current->position->x > delta)
    {
        return false;
    }
    if (current->position->y - other->position->y > delta &&
        other->position->y - current->position->y > delta)
    {
        return false;
    }
    if (current->position->z - other->position->z > delta &&
        other->position->z - current->position->z > delta)
    {
        return false;
    }
    return true;
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests basic functionality of Kineamtic Solver
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(pleaseWorkTest)
{
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
    vector<double> v = {2, 3, 4, 5, 0, 1, 2, 3, 3, 4};
    ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"), v);
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests whether the KinematicSolver is really a singleton so there is no Problem instantiating the instance
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(singletonTest)
{
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    std::cout << robot->getRobotNodeSet("TorsoRightArm")->getAllRobotNodes().size();
    KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
    ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
                            {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
    KinematicSolverPtr ks2 = KinematicSolver::getInstance(NULL, NULL);
    ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
                            {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
    KinematicSolverPtr ks3 = KinematicSolver::getInstance(NULL, robot);
    ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
                            {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
    KinematicSolver::reset();
    //shouldnt crash after reset
    ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
                            {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests whether the calculated Joint Angles are really there to reach the desired Pose and not a totally different
/// Pose; furhermore tests whether doForwardKinematic is really the inverse function of the IKSolving functions
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(backAndForthTest)
{
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
    PoseBasePtr pose = PoseBasePtr(
        new Pose(new Vector3(-300, 750, 1250), OrientationConversion::toQuaternion(0, 0, 0)));
    PoseBasePtr result =
        ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"),
                                ks1->solveIK(robot->getRobotNodeSet("TorsoLeftArm"),
                                             pose,
                                             IKSolver::CartesianSelection::Position,
                                             50));
    BOOST_CHECK_CLOSE(pose->position->x, result->position->x, 1);
    BOOST_CHECK_CLOSE(pose->position->y, result->position->y, 1);
    BOOST_CHECK_CLOSE(pose->position->z, result->position->z, 1);
 
    /*
    //ks1->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), robot->getRobotNodeSet("TorsoLeftArm")->getJointValues() , pose, IKSolver::CartesianSelection::Position);
    //result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"),);
    BOOST_CHECK_CLOSE(pose->position->x, result->position->x, 1);
    BOOST_CHECK_CLOSE(pose->position->y, result->position->y, 1);
    BOOST_CHECK_CLOSE(pose->position->z, result->position->z, 1);
 
    //result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"), ks1->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), robot->getRobotNodeSet("TorsoLeftArm")->getJointValues() , pose, IKSolver::CartesianSelection::Position));
    BOOST_CHECK_EQUAL(pose->position->x, result->position->x);
    BOOST_CHECK_EQUAL(pose->position->y, result->position->y);
    BOOST_CHECK_EQUAL(pose->position->z, result->position->z);
 
    //result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"), ks1->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), robot->getRobotNodeSet("TorsoLeftArm")->getJointValues() , pose, IKSolver::CartesianSelection::Position));
    BOOST_CHECK_EQUAL(pose->position->x, result->position->x);
    BOOST_CHECK_EQUAL(pose->position->y, result->position->y);
    BOOST_CHECK_EQUAL(pose->position->z, result->position->z);*/
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for linear
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(recursiveIKLinear)
{
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    Vector3BasePtr pos1 = new Vector3(-100, 700, 1000);
    QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 1, 1);
    PoseBasePtr pose1 = new Pose(pos1, ori1);
 
    Vector3BasePtr pos2 = new Vector3(0, 600, 1400);
    QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(0, 0, 0);
    PoseBasePtr pose2 = new Pose(pos2, ori2);
 
 
    Vector3BasePtr pos3 = new Vector3(-250, 700, 1350);
    QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(0, 0, 0);
    PoseBasePtr pose3 = new Pose(pos3, ori3);
 
    std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
    LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
 
 
    std::vector<PoseBasePtr> myPoints = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (double d = 0; d < 1; d = d + 0.01f)
    {
        myPoints.push_back(ip->getPoseAt(d));
        mySelection.push_back(IKSolver::CartesianSelection::All);
    }
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"),
                             ks->solveIK(robot->getRobotNodeSet("TorsoLeftArm"),
                                         pose1,
                                         IKSolver::CartesianSelection::Position,
                                         50),
                             myPoints,
                             mySelection);
    /*for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            //std::cout << std::to_string(angle) + ",";
        }
        //std::cout << "----------------------------------\n";*/
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*REACHABLE START
 
{JOINTANGLES
{
  "Shoulder 1 L": 0.464789,
  "Shoulder 2 L": 0.357171,
  "Upperarm L": -0.303694,
  "Elbow L": -0.067161,
  "Underarm L": -0.445988,
  "Wrist 1 L": -0.204292,
  "Wrist 2 L": -0.382231
}
}
{GLOBALPOSE    ,
{
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.4438760876655579,
   "qx" : -0.4535938203334808,
   "qy" : 0.5431225895881653,
   "qz" : 0.5497677922248840,
   "type" : "::armarx::FramedPoseBase",
   "x" : -493.14355468750,
   "y" : 415.3535156250,
   "z" : 1071.592285156250
}
 
}
 
}
----------------------------------------------------------------------------------------
REACHABLE MIDDLE
 
{
  "Shoulder 1 L": -0.147836,
  "Shoulder 2 L": -0.132739,
  "Upperarm L": 0.418313,
  "Elbow L": -0.248675,
  "Underarm L": 0.039376,
  "Wrist 1 L": 0.385483,
  "Wrist 2 L": -0.070153
}
 
{
   "qw" : 0.4338901340961456,
   "qx" : -0.4268467724323273,
   "qy" : 0.5642792582511902,
   "qz" : 0.5577904582023621,
   "x" : -182.23925781250,
   "y" : 580.074218750,
   "z" : 1034.98925781250
}
---------------------------------------------------------------------------------------------
REACHABLE END
{
  "Shoulder 1 L": -0.196959,
  "Shoulder 2 L": -0.096939,
  "Upperarm L": 0.202604,
  "Elbow L": 0.307059,
  "Underarm L": 0.128317,
  "Wrist 1 L": 0.105766,
  "Wrist 2 L": 0.513981
}
 
{
   "qw" : 0.4336481690406799,
   "qx" : -0.4273631870746613,
   "qy" : 0.5638203620910645,
   "qz" : 0.5580471754074097,
   "x" : -226.792480468750,
   "y" : 580.723144531250,
   "z" : 1186.157348632812
}---------------------------------------------------------------------------------------------
*/
BOOST_AUTO_TEST_CASE(recursiveIKThreeCP)
{
    std::vector<double> ja = {
        0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
    PosePkg p1 = createPosePkg(-348.304718,
                               580.476440,
                               712.264465,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //reachable
    PosePkg p2 = createPosePkg(-182.23925781250,
                               580.074218750,
                               1034.98925781250,
                               0.4338901340961456,
                               -0.4268467724323273,
                               0.5642792582511902,
                               0.5577904582023621); //reachable
    PosePkg p3 = createPosePkg(-226.792480468750,
                               580.723144531250,
                               1186.157348632812,
                               0.4336481690406799,
                               -0.4273631870746613,
                               0.5638203620910645,
                               0.5580471754074097); //reachable
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments(
            {p1.pose, p2.pose, p3.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 100; i++)
        {
            poses.push_back(current->getPoseAt(i / 100.0));
            std::cout << std::to_string(current->getPoseAt(i / 100.0)->position->x) + "\n";
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
    for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            std::cout << std::to_string(angle) + ",";
        }
        //std::cout << "----------------------------------\n";
    }
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
{
  "Shoulder 1 L": -0.818259,
  "Shoulder 2 L": 0.428277,
  "Upperarm L": 0.136299,
  "Elbow L": -0.608422,
  "Underarm L": 1.064518,
  "Wrist 1 L": -0.227969,
  "Wrist 2 L": -0.289433
 
{
   "agent" : "Armar3_2",
   "frame" : "Armar3_Base",
   "qw" : -0.03198593109846115,
   "qx" : 0.01920612715184689,
   "qy" : 0.7076730132102966,
   "qz" : 0.7055543065071106,
   "type" : "::armarx::FramedPoseBase",
   "x" : -502.756347656250,
   "y" : 702.6647949218750,
   "z" : 1286.8144531250
}
-----------------------------------------------------------------
{
  "Shoulder 1 L": -0.656211,
  "Shoulder 2 L": 0.329582,
  "Upperarm L": 0.476974,
  "Elbow L": 0.139211,
  "Underarm L": 2.697165,
  "Wrist 1 L": -0.446896,
  "Wrist 2 L": -0.089351
}
 
{
   "agent" : "Armar3_2",
   "frame" : "Armar3_Base",
   "qw" : 0.6405168175697327,
   "qx" : -0.3970025181770325,
   "qy" : -0.3363495767116547,
   "qz" : -0.5647976398468018,
   "type" : "::armarx::FramedPoseBase",
   "x" : -290.5917968750,
   "y" : 636.383300781250,
   "z" : 1363.315063476562
}
-----------------------------------------------------------------
{
  "Shoulder 1 L": -0.963948,
  "Shoulder 2 L": -0.103554,
  "Upperarm L": -0.255582,
  "Elbow L": -0.983395,
  "Underarm L": -0.254656,
  "Wrist 1 L": -0.458602,
  "Wrist 2 L": 0.268590
}
 
{
   "agent" : "Armar3_2",
   "frame" : "Armar3_Base",
   "qw" : 0.5907033681869507,
   "qx" : -0.5503066778182983,
   "qy" : 0.4992305040359497,
   "qz" : 0.3146440684795380,
   "type" : "::armarx::FramedPoseBase",
   "x" : -316.302246093750,
   "y" : 777.949218750,
   "z" : 1194.246459960938
}
*/
BOOST_AUTO_TEST_CASE(recursiveIKThreeCPWithRotation)
{
    std::vector<double> ja = {
        -0.818259, 0.428277, 0.136299, -0.608422, 1.064518, -0.227969, -0.289433};
    PosePkg p1 = createPosePkg(-337.719116,
                               927.767395,
                               907.007935,
                               -0.03198593109846115,
                               0.01920612715184689,
                               0.7076730132102966,
                               0.7055543065071106);
    PosePkg p2 = createPosePkg(-290.5917968750,
                               636.383300781250,
                               1363.315063476562,
                               0.6405168175697327,
                               -0.3970025181770325,
                               -0.3363495767116547,
                               -0.5647976398468018);
    PosePkg p3 = createPosePkg(-316.302246093750,
                               777.949218750,
                               1194.246459960938,
                               0.5907033681869507,
                               -0.5503066778182983,
                               0.4992305040359497,
                               0.3146440684795380);
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments(
            {p1.pose, p2.pose, p3.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 50; i++)
        {
            poses.push_back(current->getPoseAt(i / 50.0));
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
    //std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
    for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            std::cout << std::to_string(angle) + ",";
        }
        std::cout << "----------------------------------\n";
    }
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*REACHABLE START
 
UNREACHABLE START
 
{
  "Shoulder 1 L": -0.248207,
  "Shoulder 2 L": -0.235603,
  "Upperarm L": 0.130206,
  "Elbow L": -1.000947,
  "Underarm L": 0.057843,
  "Wrist 1 L": 0.197757,
  "Wrist 2 L": -0.663139
}
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.4207711517810822,
   "qx" : -0.4548574388027191,
   "qy" : 0.5738259553909302,
   "qz" : 0.5355183482170105,
   "type" : "::armarx::FramedPoseBase",
   "x" : -234.163574218750,
   "y" : 562.9924316406250,
   "z" : 902.9812011718750
}
-----------------------------------------------------------------
{
  "Shoulder 1 L": -0.763830,
  "Shoulder 2 L": -0.244323,
  "Upperarm L": 0.619645,
  "Elbow L": -0.281197,
  "Underarm L": 0.040509,
  "Wrist 1 L": 0.444159,
  "Wrist 2 L": 0.536926
}
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.4365486800670624,
   "qx" : -0.4239800870418549,
   "qy" : 0.5666229724884033,
   "qz" : 0.5555219650268555,
   "type" : "::armarx::FramedPoseBase",
   "x" : -95.164550781250,
   "y" : 705.6569824218750,
   "z" : 1245.026000976562
}---------------------------------------------------------------------------------------------
*/
BOOST_AUTO_TEST_CASE(unreachableStart)
{
    std::vector<double> ja = {
        0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
    PosePkg p1 = createPosePkg(-348.304718,
                               580.476440,
                               712.264465,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //unreachable
    PosePkg p2 = createPosePkg(-182.23925781250,
                               580.074218750,
                               1034.98925781250,
                               0.4338901340961456,
                               -0.4268467724323273,
                               0.5642792582511902,
                               0.5577904582023621); //reachable
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 50; i++)
        {
            poses.push_back(current->getPoseAt(i / 50.0));
            //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
    for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            std::cout << std::to_string(angle) + ",";
        }
        std::cout << "----------------------------------\n";
    }
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 
UNREACHABLE END
 
{
  "Shoulder 1 L": -0.763830,
  "Shoulder 2 L": -0.244323,
  "Upperarm L": 0.619645,
  "Elbow L": -0.281197,
  "Underarm L": 0.040509,
  "Wrist 1 L": 0.444159,
  "Wrist 2 L": 0.536926
}
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.4365486800670624,
   "qx" : -0.4239800870418549,
   "qy" : 0.5666229724884033,
   "qz" : 0.5555219650268555,
   "type" : "::armarx::FramedPoseBase",
   "x" : -95.164550781250,
   "y" : 705.6569824218750,
   "z" : 1245.026000976562
}
-----------------------------------------------------------------
{
  "Shoulder 1 L": -1.132457,
  "Shoulder 2 L": -0.244346,
  "Upperarm L": 1.040296,
  "Elbow L": -0.892947,
  "Underarm L": -0.627532,
  "Wrist 1 L": 0.523122,
  "Wrist 2 L": 0.100948
}
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.4369190335273743,
   "qx" : -0.4234801232814789,
   "qy" : 0.5673816800117493,
   "qz" : 0.5548373460769653,
   "type" : "::armarx::FramedPoseBase",
   "x" : -93.015136718750,
   "y" : 784.5754394531250,
   "z" : 1246.448608398438
}---------------------------------------------------------------------------------------------
*/
BOOST_AUTO_TEST_CASE(UnreachableEnd)
{
    std::vector<double> ja = {
        -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
    PosePkg p1 = createPosePkg(4.825809,
                               788.516541,
                               1083.528442,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //reachable
    PosePkg p2 = createPosePkg(-93.015136718750,
                               784.5754394531250,
                               500.448608398438,
                               0.4338901340961456,
                               -0.4268467724323273,
                               0.5642792582511902,
                               0.5577904582023621); //unreachable
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 50; i++)
        {
            poses.push_back(current->getPoseAt(i / 50.0));
            std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
    //std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
    //BOOST_CHECK_THROW(ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection), armarx::LocalException);
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*REACHABLE START
 
ENREACHABLE MID
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.4367099106311798,
   "qx" : -0.4243824779987335,
   "qy" : 0.5662068724632263,
   "qz" : 0.5555122494697571,
   "type" : "::armarx::FramedPoseBase",
   "x" : -112.750,
   "y" : 696.7272949218750,
   "z" : 1181.076660156250
}
 
{
  "Shoulder 1 L": -0.663384,
  "Shoulder 2 L": -0.188166,
  "Upperarm L": 0.658974,
  "Elbow L": -0.346670,
  "Underarm L": -0.050623,
  "Wrist 1 L": 0.490788,
  "Wrist 2 L": 0.335159
}
-----------------------------------------------------------------
{
  "Shoulder 1 L": -1.346342,
  "Shoulder 2 L": -0.244346,
  "Upperarm L": 0.968929,
  "Elbow L": -0.641009,
  "Underarm L": -0.288792,
  "Wrist 1 L": 0.454785,
  "Wrist 2 L": 0.657779
}
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : 0.3848522007465363,
   "qx" : -0.3703520894050598,
   "qy" : 0.6027946472167969,
   "qz" : 0.5927618741989136,
   "type" : "::armarx::FramedPoseBase",
   "x" : -64.90527343750,
   "y" : 766.2385253906250,
   "z" : 1396.328857421875
}
-----------------------------------------------------------------
{
  "Shoulder 1 L": -1.531028,
  "Shoulder 2 L": -0.213389,
  "Upperarm L": 1.010730,
  "Elbow L": -0.683772,
  "Underarm L": 0.458575,
  "Wrist 1 L": -0.436870,
  "Wrist 2 L": 0.699134
}
 
{
   "agent" : "Armar3_1",
   "frame" : "Armar3_Base",
   "qw" : -0.0135999433696270,
   "qx" : 0.03095184452831745,
   "qy" : 0.7067553400993347,
   "qz" : 0.7066496610641479,
   "type" : "::armarx::FramedPoseBase",
   "x" : -79.7832031250,
   "y" : 766.7092285156250,
   "z" : 1471.756713867188
}---------------------------------------------------------------------------------------------
*/
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(unreachableMid)
{
    std::vector<double> ja = {
        -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
    PosePkg p1 = createPosePkg(4.825809,
                               788.516541,
                               1083.528442,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //reachable
    PosePkg p2 = createPosePkg(-93.015136718750,
                               784.5754394531250,
                               500.448608398438,
                               0.4338901340961456,
                               -0.4268467724323273,
                               0.5642792582511902,
                               0.5577904582023621); //unreachable
    PosePkg p3 = createPosePkg(4.825809,
                               788.516541,
                               1083.528442,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //reachable
 
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments(
            {p1.pose, p2.pose, p3.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 50; i++)
        {
            poses.push_back(current->getPoseAt(i / 50.0));
            //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
    //std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
    //BOOST_CHECK_THROW(ks->solveRecursiveIKRelative(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection), armarx::LocalException);
}
 
/*
//COLLSIONSTART
{
GlobalPose
-0.022014  -0.469058   0.882893    56.5798
-0.0480052   0.882584   0.467697    233.545
 -0.998605 -0.0320876 -0.0419464    992.508
         0          0          0          1
}
{
0.315176
-0.0275324
1.00421
0.0182586
0.127499
-0.0255035
-0.299747
}
-----------------------------------------------
{
GlobalPose
-0.567009    -0.7504   0.339706   -95.4907
  0.148646   0.312423   0.938241    761.478
 -0.810188   0.582488 -0.0656025    1208.03
         0          0          0          1
}
{
-0.890715
-0.244346
0.487422
-0.723896
0.232289
-0.0929337
0.130688
*/
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(collisionStart)
{
    std::vector<double> ja = {
        0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
    PosePkg p1 = createPosePkg(56.5798,
                               233.545,
                               992.508,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //Colliding Pose
    PosePkg p2 = createPosePkg(-95.4907,
                               761.478,
                               1208.03,
                               0.4338901340961456,
                               -0.4268467724323273,
                               0.5642792582511902,
                               0.5577904582023621); //Not Colliding Pose
 
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 50; i++)
        {
            poses.push_back(current->getPoseAt(i / 50.0));
            //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematic(robot->getRobotNodeSet("LeftArm"), ja);
    std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," +
                     std::to_string(result2->position->y) + "," +
                     std::to_string(result2->position->z) + "\n";
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
    /*for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            //std::cout << std::to_string(angle) + ",";
        }
        std::cout << "----------------------------------\n";
    }*/
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 * // START
 Global Pose
-0.471909 -0.163874  0.866284   3.02088
  0.39698  0.837837  0.374748   389.723
-0.787216  0.520744 -0.330328   938.033
        0         0         0         1
JointAngles
-0.0475329
-0.0275045
0.790436
-0.205607
0.561071
-0.322912
-0.256206
 
//END
 
Global Pose
-0.222048  0.721908  0.655395  -167.276
0.00806859  0.673513 -0.739131  -187.905
-0.975002 -0.158835 -0.155377   999.493
        0         0         0         1
jointAngles
0.733038
0.535259
1.22173
0.0361974
0.991169
-0.506145
-0.663225
 *
 *
}*/
BOOST_AUTO_TEST_CASE(CollisonMid)
{
    std::vector<double> ja = {
        -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
    PosePkg p1 = createPosePkg(3.02088,
                               389.723,
                               938.033,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //no collision here
    //in between there is a collision with robot body
    PosePkg p2 = createPosePkg(-167.276,
                               -187.905,
                               999.493,
                               0.4338901340961456,
                               -0.4268467724323273,
                               0.5642792582511902,
                               0.5577904582023621); //no one here either
 
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (AbstractInterpolationPtr current : ip)
    {
        for (int i = 0; i < 50; i++)
        {
            poses.push_back(current->getPoseAt(i / 50.0));
            //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
            mySelection.push_back(IKSolver::CartesianSelection::Position);
        }
    }
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
    /*for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            //std::cout << std::to_string(angle) + ",";
        }
        std::cout << "----------------------------------\n";
    }*/
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// No Movement
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(NoMovement)
{
    std::cout << "NO MOVEMENT START";
    std::vector<double> ja = {
        -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
    PosePkg p1 = createPosePkg(3.02088,
                               389.723,
                               938.033,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //no collision here
    PosePkg p2 = createPosePkg(3.02088,
                               389.723,
                               938.033,
                               0.4453609585762024,
                               -0.4538218379020691,
                               0.5429607033729553,
                               0.5485371351242065); //no collision here
 
    std::vector<PoseBasePtr> cp = {p1.pose, p2.pose};
    LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
    std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
    std::vector<IKSolver::CartesianSelection> mySelection =
        std::vector<IKSolver::CartesianSelection>();
    for (int i = 0; i < 50; i++)
    {
        poses.push_back(ip->getPoseAt(i / 50.0));
        std::cout << std::to_string(ip->getPoseAt(i / 50.0)->position->x) + "\n";
        mySelection.push_back(IKSolver::CartesianSelection::Position);
    }
 
 
    RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    //Look whther first Pose is reachable
    std::vector<std::vector<double>> result =
        ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
    for (vector<double> tempResult : result)
    {
        for (double angle : tempResult)
        {
            std::cout << std::to_string(angle) + ",";
        }
        std::cout << "----------------------------------\n";
    }
    std::cout << "NO MOVEMENT END";
}