UserWaypointTests.cpp

Go to the documentation of this file.

#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::UserWaypoint
#define ARMARX_BOOST_TEST
 
 
#include <RobotComponents/Test.h>
 
#include "../UserWaypoint.h"
#include "../Util/OrientationConversion.h"
 
using namespace armarx;
 
BOOST_AUTO_TEST_CASE(basicTest)
{
    // consturct a UserWaypoint and tests getter
 
    PoseBasePtr pose1 = PoseBasePtr(new Pose());
    UserWaypoint w1 = UserWaypoint(pose1);
    std::vector<double> jointAngles = {1.0, 2.0, 3.0};
 
    //check constructor
    BOOST_CHECK_EQUAL(w1.getTimeOptimalTimestamp(), 0);
    BOOST_CHECK_EQUAL(w1.getUserTimestamp(), 0);
    BOOST_CHECK_EQUAL(w1.getIKSelection(), VirtualRobot::IKSolver::CartesianSelection::All);
    BOOST_CHECK_EQUAL(w1.getIsTimeOptimalBreakpoint(), false);
 
 
    //check getPose
    BOOST_CHECK_EQUAL(w1.getPose(), pose1);
 
    //check setJointAngles and getJointAngles
    w1.setJointAngles(jointAngles);
    BOOST_CHECK_EQUAL(w1.getJointAngles(), jointAngles);
 
    //check setIKSelection and getIKSelection
 
    //check getIsTimeOptimalBreakpoint
    w1.setIsTimeOptimalBreakpoint(true);
    BOOST_CHECK_EQUAL(w1.getIsTimeOptimalBreakpoint(), true);
 
    //check get/set TimeOptimalTimestamp/UserTimestamp
    w1.setUserTimestamp(10);
    BOOST_CHECK_EQUAL(w1.getUserTimestamp(), 10);
    w1.setTimeOptimalTimestamp(20);
    BOOST_CHECK_EQUAL(w1.getTimeOptimalTimestamp(), 20);
    BOOST_CHECK_EQUAL(w1.getUserTimestamp(), 20);
    w1.setUserTimestamp(30);
    BOOST_CHECK_EQUAL(w1.getUserTimestamp(), 30);
    BOOST_CHECK_EQUAL(w1.getTimeOptimalTimestamp(), 20);
    //UserTime lower than timeoptimal
    BOOST_CHECK_THROW(w1.setUserTimestamp(10), InvalidArgumentException);
 
 
    //wrong usage of setter
    BOOST_CHECK_THROW(w1.setUserTimestamp(-10), InvalidArgumentException);
    BOOST_CHECK_THROW(w1.setTimeOptimalTimestamp(-10), InvalidArgumentException);
    BOOST_CHECK_THROW(w1.setJointAngles(std::vector<double>()), InvalidArgumentException);
}
 
BOOST_AUTO_TEST_CASE(deepCopyTest)
{
    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));
 
 
    UserWaypoint w1 = UserWaypoint(pose1);
    std::vector<double> jointAngles1 = {1.0, 2.0, 3.0};
    std::vector<double> jointAngles2 = {4.0, 5.0, 6.0};
    double userT1 = 10;
    double userT2 = 20;
    double optimalT1 = 5;
    double optimalT2 = 10;
 
    w1.setJointAngles(jointAngles1);
    w1.setIsTimeOptimalBreakpoint(true);
    w1.setUserTimestamp(userT1);
    w1.setTimeOptimalTimestamp(optimalT1);
 
    //copy of w1
    UserWaypoint w2 = UserWaypoint(w1);
 
    BOOST_CHECK_EQUAL(w2.getJointAngles(), jointAngles1);
    BOOST_CHECK_EQUAL(w2.getIsTimeOptimalBreakpoint(), true);
    BOOST_CHECK_EQUAL(w2.getUserTimestamp(), userT1);
    BOOST_CHECK_EQUAL(w2.getTimeOptimalTimestamp(), optimalT1);
    //BOOST_CHECK_EQUAL(*w2.getPose(), *pose1);
 
    //change w2
    w2.setJointAngles(jointAngles2);
    w2.setIsTimeOptimalBreakpoint(false);
    w2.setUserTimestamp(userT2);
    w2.setTimeOptimalTimestamp(optimalT2);
    w2.setPose(pose2);
 
    //check w1 and w2
    BOOST_CHECK_EQUAL(w2.getJointAngles(), jointAngles2);
    BOOST_CHECK_EQUAL(w2.getIsTimeOptimalBreakpoint(), false);
    BOOST_CHECK_EQUAL(w2.getUserTimestamp(), userT2);
    BOOST_CHECK_EQUAL(w2.getTimeOptimalTimestamp(), optimalT2);
    BOOST_CHECK_EQUAL(w2.getPose(), pose2);
 
    BOOST_CHECK_EQUAL(w1.getJointAngles(), jointAngles1);
    BOOST_CHECK_EQUAL(w1.getIsTimeOptimalBreakpoint(), true);
    BOOST_CHECK_EQUAL(w1.getUserTimestamp(), userT1);
    BOOST_CHECK_EQUAL(w1.getTimeOptimalTimestamp(), optimalT1);
    BOOST_CHECK_EQUAL(w1.getPose(), pose1);
}