DesignerTrajectoryTests.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectory
#define ARMARX_BOOST_TEST
 
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotComponents/Test.h>
 
#include "../DesignerTrajectory.h"
#include "../Util/OrientationConversion.h"
 
using namespace armarx;
 
namespace
{
    bool
    equalPose(PoseBasePtr p1, PoseBasePtr p2)
    {
        QuaternionPtr q1 = QuaternionPtr::dynamicCast(p1->orientation);
        QuaternionPtr q2 = QuaternionPtr::dynamicCast(p2->orientation);
        Vector3Ptr pos1 = Vector3Ptr::dynamicCast(p1->position);
        Vector3Ptr pos2 = Vector3Ptr::dynamicCast(p2->position);
 
        if (q1->toEigen() == q2->toEigen() && pos1->toEigen() == pos2->toEigen())
        {
            return true;
        }
        else
        {
            return false;
        }
    }
 
    bool
    equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
    {
        return (w1->getIKSelection() == w2->getIKSelection() &&
                w1->getIsTimeOptimalBreakpoint() == w2->getIsTimeOptimalBreakpoint() &&
                w1->getJointAngles() == w2->getJointAngles() &&
                equalPose(w1->getPose(), w2->getPose()) &&
                w1->getTimeOptimalTimestamp() == w1->getTimeOptimalTimestamp() &&
                w1->getUserTimestamp() == w2->getUserTimestamp())
                   ? true
                   : false;
    }
 
 
} // namespace
 
BOOST_AUTO_TEST_CASE(basicTest)
{
    //Init
    Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
    QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
    PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
 
    Vector3BasePtr pos2 = Vector3BasePtr(new Vector3(4, 5, 6));
    QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(4, 5, 6);
    PoseBasePtr pose2 = PoseBasePtr(new Pose(pos2, ori2));
 
    Vector3BasePtr pos3 = Vector3BasePtr(new Vector3(7, 8, 9));
    QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(7, 8, 9);
    PoseBasePtr pose3 = PoseBasePtr(new Pose(pos3, ori3));
 
    Vector3BasePtr pos4 = Vector3BasePtr(new Vector3(10, 11, 12));
    QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(10, 11, 12);
    PoseBasePtr pose4 = PoseBasePtr(new Pose(pos4, ori4));
 
    UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
    w1->setJointAngles({1, 2, 3, 4, 5, 6, 7});
    UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
    UserWaypointPtr w3 = UserWaypointPtr(new UserWaypoint(pose3));
    UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
 
    VirtualRobot::RobotPtr robot;
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
 
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
 
    VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
 
    w1->setUserTimestamp(10);
    w2->setUserTimestamp(20);
    w3->setUserTimestamp(30);
 
    DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
 
    BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 1);
 
    //Wrong usage
    BOOST_CHECK_THROW(dt1.getUserWaypoint(2), IndexOutOfBoundsException);
    BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException);
    BOOST_CHECK_THROW(dt1.getTrajectorySegment(2), IndexOutOfBoundsException);
 
    //add UserWaypoints and test Transition
    dt1.addFirstUserWaypoint(w2);
    dt1.insertUserWaypoint(w3, 1);
    dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
 
    BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
    BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w3);
    BOOST_CHECK_EQUAL(dt1.getTransition(1)->getStart(), w3);
    BOOST_CHECK_EQUAL(dt1.getTransition(1)->getEnd(), w1);
    BOOST_CHECK_EQUAL(dt1.getTransition(2)->getStart(), w1);
    BOOST_CHECK_EQUAL(dt1.getTransition(2)->getEnd(), w4);
 
    //delete UserWaypoint
    dt1.deleteUserWaypoint(1);
    BOOST_CHECK_THROW(dt1.getTransition(2),
                      IndexOutOfBoundsException); //there should be not three transitions
    BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
    BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w1);
    BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 3);
}
 
BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
{
    //Init Designer Trajectory
    Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
    QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
    PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
    UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
    w1->setJointAngles({1, 2, 3, 4, 5, 6, 7});
 
    // Get Robot
    std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
    CMakePackageFinder finder("RobotAPI");
    VirtualRobot::RobotPtr robot;
    if (finder.packageFound())
    {
        robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
    }
    else
    {
        robot = VirtualRobot::RobotIO::loadRobot(
            "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
    }
    VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
    DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
 
    //Init three Trajectories
    std::vector<std::vector<double>> data1 = {{1, 2, 3, 4, 5},
                                              {1, 2, 3, 4, 5},
                                              {1, 2, 3, 4, 5},
                                              {1, 2, 3, 4, 5},
                                              {1, 2, 3, 4, 5},
                                              {1, 2, 3, 4, 5},
                                              {1, 2, 3, 4, 5}};
    std::vector<std::vector<double>> data2 = {{5, 6, 7, 8, 9},
                                              {5, 6, 7, 8, 9},
                                              {5, 6, 7, 8, 9},
                                              {5, 6, 7, 8, 9},
                                              {5, 6, 7, 8, 9},
                                              {5, 6, 7, 8, 9},
                                              {5, 6, 7, 8, 9}};
    std::vector<std::vector<double>> data3 = {{9, 10, 11, 12, 13},
                                              {9, 10, 11, 12, 13},
                                              {9, 10, 11, 12, 13},
                                              {9, 10, 11, 12, 13},
                                              {9, 10, 11, 12, 13},
                                              {9, 10, 11, 12, 13},
                                              {9, 10, 11, 12, 13}};
 
    Ice::DoubleSeq timestamps1 = {0, 1, 2, 3, 4};
    Ice::DoubleSeq timestamps2 = {0, 1, 2, 3, 4};
    Ice::DoubleSeq timestamps3 = {0, 1, 2, 3, 4};
    Ice::StringSeq dimensionNames = {"a", "b", "c", "d", "e", "f", "g"};
 
    TrajectoryPtr traj1(new Trajectory(data1, timestamps1, dimensionNames));
    TrajectoryPtr traj2(new Trajectory(data2, timestamps2, dimensionNames));
    TrajectoryPtr traj3(new Trajectory(data3, timestamps3, dimensionNames));
    /////////////////////////////////////////////////////////////////////////////////////
 
    dt1.setInterBreakpointTrajectories({traj1, traj2, traj3});
    BOOST_CHECK_EQUAL(dt1.getInterBreakpointTrajectories().size(), 3);
 
 
    std::vector<std::vector<double>> data4 = {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
                                              {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
                                              {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
                                              {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
                                              {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
                                              {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
                                              {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}};
 
    Ice::DoubleSeq timestamps4 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
    TrajectoryPtr traj4(new Trajectory(data4, timestamps4, dimensionNames));
 
    TrajectoryPtr traj5 = dt1.getTimeOptimalTrajectory();
    // Traj5 should be same as traj4
    //check every dimension
    BOOST_CHECK_EQUAL(traj5->getDimensionData(0), traj4->getDimensionData(0));
    BOOST_CHECK_EQUAL(traj5->getDimensionData(1), traj4->getDimensionData(1));
    BOOST_CHECK_EQUAL(traj5->getDimensionData(2), traj4->getDimensionData(2));
    BOOST_CHECK_EQUAL(traj5->getDimensionData(3), traj4->getDimensionData(3));
    BOOST_CHECK_EQUAL(traj5->getDimensionData(4), traj4->getDimensionData(4));
    BOOST_CHECK_EQUAL(traj5->getDimensionData(5), traj4->getDimensionData(5));
    BOOST_CHECK_EQUAL(traj5->getDimensionData(6), traj4->getDimensionData(6));
    //check timestamps
    BOOST_CHECK_EQUAL(traj5->getTimestamps(), traj4->getTimestamps());
 
    //checkDerivations
    BOOST_CHECK_EQUAL(traj5->getDerivations(1, 0, 2), traj4->getDerivations(1, 0, 2));
    BOOST_CHECK_EQUAL(traj5->getDerivations(3, 0, 2), traj4->getDerivations(3, 0, 2));
    BOOST_CHECK_EQUAL(traj5->getDerivations(5, 0, 2), traj4->getDerivations(5, 0, 2));
    BOOST_CHECK_EQUAL(traj5->getDerivations(7, 0, 2), traj4->getDerivations(7, 0, 2));
    BOOST_CHECK_EQUAL(traj5->getDerivations(9, 0, 2), traj4->getDerivations(9, 0, 2));
    BOOST_CHECK_EQUAL(traj5->getDerivations(11, 0, 2), traj4->getDerivations(11, 0, 2));
}
 
BOOST_AUTO_TEST_CASE(getFinalTraj)
{
}