TransitionTests.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Transition
#define ARMARX_BOOST_TEST
 
 
#include <RobotComponents/Test.h>
 
#include "../Transition.h"
#include "../Util/OrientationConversion.h"
 
using namespace armarx;
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));
 
    UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
    UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
 
 
    //Consturctor test
    Transition t1 = Transition(w1, w2);
    BOOST_CHECK_EQUAL(t1.getStart(), w1);
    BOOST_CHECK_EQUAL(t1.getEnd(), w2);
    BOOST_CHECK_EQUAL(t1.getInterpolationType(), eLinearInterpolation);
    BOOST_CHECK_EQUAL(t1.getTimeOptimalDuration(), 0);
    BOOST_CHECK_EQUAL(t1.getUserDuration(), 0);
 
    //check user/timeOptimal duration
    t1.setUserDuration(5);
    BOOST_CHECK_EQUAL(t1.getUserDuration(), 5);
    t1.setTimeOptimalDuration(10);
    BOOST_CHECK_EQUAL(t1.getTimeOptimalDuration(), 10);
    BOOST_CHECK_EQUAL(t1.getUserDuration(), 10);
    //UserDuration lower Than timeOptimal
    BOOST_CHECK_THROW(t1.setUserDuration(5), InvalidArgumentException);
 
    //wrong usage
    BOOST_CHECK_THROW(t1.setUserDuration(-1), InvalidArgumentException);
    BOOST_CHECK_THROW(t1.setTimeOptimalDuration(-1), InvalidArgumentException);
}
 
 
BOOST_AUTO_TEST_CASE(deepCopyTest)
{
    //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));
 
    UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
    UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
 
    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 w3 = UserWaypointPtr(new UserWaypoint(pose3));
    UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
 
 
    Transition t1 = Transition(w1, w2);
    t1.setTimeOptimalDuration(10);
    t1.setUserDuration(20);
 
    t1.getStart()->setTimeOptimalTimestamp(1);
    t1.getStart()->setUserTimestamp(2);
    t1.getEnd()->setTimeOptimalTimestamp(10);
    t1.getEnd()->setUserTimestamp(20);
    Transition t2 = Transition(t1, UserWaypointPtr(new UserWaypoint(*t1.getStart())),
                               UserWaypointPtr(new UserWaypoint(*t1.getEnd())));
 
 
    BOOST_CHECK_EQUAL(t2.getTimeOptimalDuration(), 10);
    BOOST_CHECK_EQUAL(t2.getUserDuration(), 20);
    BOOST_CHECK_EQUAL(t2.getStart()->getUserTimestamp(), 2);
    BOOST_CHECK_EQUAL(t2.getStart()->getTimeOptimalTimestamp(), 1);
    BOOST_CHECK_EQUAL(t2.getEnd()->getUserTimestamp(), 20);
    BOOST_CHECK_EQUAL(t2.getEnd()->getTimeOptimalTimestamp(), 10);
 
 
    t1.setTimeOptimalDuration(30);
    t1.setUserDuration(40);
    t1.getStart()->setTimeOptimalTimestamp(3);
    t1.getStart()->setUserTimestamp(4);
    t1.getEnd()->setTimeOptimalTimestamp(30);
    t1.getEnd()->setUserTimestamp(40);
 
    BOOST_CHECK_EQUAL(t2.getTimeOptimalDuration(), 10);
    BOOST_CHECK_EQUAL(t2.getUserDuration(), 20);
    BOOST_CHECK_EQUAL(t2.getStart()->getUserTimestamp(), 2);
    BOOST_CHECK_EQUAL(t2.getStart()->getTimeOptimalTimestamp(), 1);
    BOOST_CHECK_EQUAL(t2.getEnd()->getUserTimestamp(), 20);
    BOOST_CHECK_EQUAL(t2.getEnd()->getTimeOptimalTimestamp(), 10);
 
 
    BOOST_CHECK_EQUAL(t1.getTimeOptimalDuration(), 30);
    BOOST_CHECK_EQUAL(t1.getUserDuration(), 40);
    BOOST_CHECK_EQUAL(t1.getStart()->getUserTimestamp(), 4);
    BOOST_CHECK_EQUAL(t1.getStart()->getTimeOptimalTimestamp(), 3);
    BOOST_CHECK_EQUAL(t1.getEnd()->getUserTimestamp(), 40);
    BOOST_CHECK_EQUAL(t1.getEnd()->getTimeOptimalTimestamp(), 30);
}