DesignerTrajectoryHolderTest.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectoryHolder
#define ARMARX_BOOST_TEST
 
#include "../DesignerTrajectoryHolder.h"
 
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotComponents/Test.h>
 
using namespace armarx;
using namespace VirtualRobot;
 
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;
}
 
PoseBasePtr
poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string& rnsName)
{
    return PoseBasePtr(
        new Pose(robot->getRobotNodeSet(rnsName)->getKinematicRoot()->toLocalCoordinateSystem(
            (new Pose(pose->position, pose->orientation))->toEigen())));
}
 
BOOST_AUTO_TEST_CASE(basicTest)
{
    // Get Robot
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
    RobotPtr robot;
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    else
    {
        robot = RobotIO::loadRobot(
            "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
    }
 
    EnvironmentPtr environment = std::make_shared<Environment>();
    environment->setRobot(robot);
 
    // Get RobotNodeSet: LeftArm
    VirtualRobot::RobotNodeSetPtr rnsLeftArm = robot->getRobotNodeSets()[10];
 
    // Get RobotNodeSet: RightArm
    VirtualRobot::RobotNodeSetPtr rnsRightArm = robot->getRobotNodeSet("RightArm");
 
    // create designer trajectory holder
    DesignerTrajectoryHolderPtr dth = std::make_shared<DesignerTrajectoryHolder>(environment);
 
    // create designer trajectory manager of LeftArm in holder
    dth->createDesignerTrajectoryManager(rnsLeftArm->getName());
 
    BOOST_CHECK(dth->rnsExists(rnsLeftArm->getName()));
    BOOST_CHECK(!dth->rnsExists(rnsRightArm->getName()));
    BOOST_CHECK(!dth->rnsIsPartOfExistingRns(rnsRightArm->getName()));
}
 
BOOST_AUTO_TEST_CASE(collisionDetectionNoCollisionTest)
{
    // Get Robot
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
    RobotPtr robot;
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    else
    {
        robot = RobotIO::loadRobot(
            "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
    }
 
    KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
 
    EnvironmentPtr environment = std::make_shared<Environment>();
    environment->setRobot(robot);
 
    // get LeftArm
    std::string leftArmName = robot->getRobotNodeSet("LeftArm")->getName();
 
    // get LeftArmColModel
    std::string leftArmColModelName = robot->getRobotNodeSet("LeftArmColModel")->getName();
 
    // get body collision models
    std::vector<std::string> bodyColModelsNames;
    bodyColModelsNames.push_back(robot->getRobotNodeSet("TorsoHeadColModel")->getName());
 
    // create designer trajectory holder
    DesignerTrajectoryHolderPtr dth = std::make_shared<DesignerTrajectoryHolder>(environment);
 
    // create manager
    dth->createDesignerTrajectoryManager(leftArmName);
    DesignerTrajectoryManagerPtr dtm = dth->getTrajectoryManager(leftArmName);
 
 
    // Create Poses
    std::vector<double> p1ja = {
        0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
 
    PosePkg pp4 = createPosePkg(-316.302246093750,
                                777.949218750,
                                1194.246459960938,
                                0.5907033681869507,
                                -0.5503066778182983,
                                0.4992305040359497,
                                0.3146440684795380);
    PosePkg pp2 = createPosePkg(-182.23925781250,
                                580.074218750,
                                1034.98925781250,
                                0.4338901340961456,
                                -0.4268467724323273,
                                0.5642792582511902,
                                0.5577904582023621); //reachable
    PosePkg pp3 = createPosePkg(-226.792480468750,
                                580.723144531250,
                                1186.157348632812,
                                0.4336481690406799,
                                -0.4273631870746613,
                                0.5638203620910645,
                                0.5580471754074097); //reachable
    PosePkg pp5 = createPosePkg(-348.304718,
                                580.476440,
                                712.264465,
                                0.4453609585762024,
                                -0.4538218379020691,
                                0.5429607033729553,
                                0.5485371351242065); //reachable
 
    PoseBasePtr p1 = kc->doForwardKinematic(robot->getRobotNodeSet(leftArmName), p1ja);
    PoseBasePtr p2 = poseToLocal(pp2.pose, robot, leftArmName);
    //PoseBasePtr p2 = pp2.pose;
    PoseBasePtr p3 = poseToLocal(pp3.pose, robot, leftArmName);
    PoseBasePtr p4 = poseToLocal(pp4.pose, robot, leftArmName);
 
    // Add some poses to designer trajectory
    dtm->initializeDesignerTrajectory(p1ja);
    dtm->addWaypoint(p2);
    dtm->insertWaypoint(1, p3);
    dtm->insertWaypoint(0, p4);
 
    // get trajectory from designer trajectory
    armarx::TrajectoryPtr t = dtm->getDesignerTrajectory()->getFinalTrajectory();
 
    // call collision detection
    //TODO @Luca Quaer change
    BOOST_CHECK(!dth->isInCollision(leftArmColModelName, bodyColModelsNames, 10));
}
 
BOOST_AUTO_TEST_CASE(collisionDetectionCollisionTest)
{
    // Get Robot
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
    RobotPtr robot;
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    else
    {
        robot = RobotIO::loadRobot(
            "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
    }
    KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
 
    EnvironmentPtr environment = std::make_shared<Environment>();
    environment->setRobot(robot);
 
    // get LeftArm
    std::string leftArmName = robot->getRobotNodeSet("LeftArm")->getName();
 
    // get LeftArmColModel
    std::string leftArmColModelName = robot->getRobotNodeSet("LeftArmColModel")->getName();
 
    // get body collision models
    std::vector<std::string> bodyColModelsNames;
    bodyColModelsNames.push_back(robot->getRobotNodeSet("TorsoHeadColModel")->getName());
 
    // create designer trajectory holder
    DesignerTrajectoryHolderPtr dth = std::make_shared<DesignerTrajectoryHolder>(environment);
 
    // create manager
    dth->createDesignerTrajectoryManager(leftArmName);
    DesignerTrajectoryManagerPtr dtm = dth->getTrajectoryManager(leftArmName);
 
    // Create Poses
    std::vector<double> p1ja = {
        0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
    PosePkg pp1 = createPosePkg(56.5798,
                                233.545,
                                992.508,
                                0.4453609585762024,
                                -0.4538218379020691,
                                0.5429607033729553,
                                0.5485371351242065); //Colliding Pose
    PosePkg pp2 = createPosePkg(-95.4907,
                                761.478,
                                1208.03,
                                0.4338901340961456,
                                -0.4268467724323273,
                                0.5642792582511902,
                                0.5577904582023621); //Not Colliding Pose
 
    PosePkg pp9 = createPosePkg(-182.23925781250,
                                580.074218750,
                                1034.98925781250,
                                0.4338901340961456,
                                -0.4268467724323273,
                                0.5642792582511902,
                                0.5577904582023621); //reachable
 
 
    PoseBasePtr p1 = poseToLocal(pp1.pose, robot, leftArmName);
    PoseBasePtr p2 = poseToLocal(pp2.pose, robot, leftArmName);
    PoseBasePtr p9 = poseToLocal(pp9.pose, robot, leftArmName);
 
    // Add some poses to designer trajectory
    dtm->initializeDesignerTrajectory(p1ja);
    dtm->addWaypoint(p9);
 
    dtm->setTransitionUserDuration(0, 5.0);
 
    // get trajectory from designer trajectory
    armarx::TrajectoryPtr t = dtm->getDesignerTrajectory()->getFinalTrajectory();
 
    // call collision detection
    //TODO @Luca Quaer change
    BOOST_CHECK(dth->isInCollision(leftArmColModelName, bodyColModelsNames, 10));
}