DesignerTrajectoryHolderTest.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectoryHolder
#define ARMARX_BOOST_TEST
 
#include <RobotComponents/Test.h>
 
 
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include "../DesignerTrajectoryHolder.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));
}