TransitionTests.cpp
Go to the documentation of this file.
1 #define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Transition
2 #define ARMARX_BOOST_TEST
3 
4 
5 #include <RobotComponents/Test.h>
6 
7 #include "../Transition.h"
8 #include "../Util/OrientationConversion.h"
9 
10 using namespace armarx;
11 
13 {
14  //Init
15  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
16  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
17  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
18 
19  Vector3BasePtr pos2 = Vector3BasePtr(new Vector3(4, 5, 6));
20  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(4, 5, 6);
21  PoseBasePtr pose2 = PoseBasePtr(new Pose(pos2, ori2));
22 
25 
26 
27  //Consturctor test
28  Transition t1 = Transition(w1, w2);
29  BOOST_CHECK_EQUAL(t1.getStart(), w1);
30  BOOST_CHECK_EQUAL(t1.getEnd(), w2);
31  BOOST_CHECK_EQUAL(t1.getInterpolationType(), eLinearInterpolation);
32  BOOST_CHECK_EQUAL(t1.getTimeOptimalDuration(), 0);
33  BOOST_CHECK_EQUAL(t1.getUserDuration(), 0);
34 
35  //check user/timeOptimal duration
36  t1.setUserDuration(5);
37  BOOST_CHECK_EQUAL(t1.getUserDuration(), 5);
39  BOOST_CHECK_EQUAL(t1.getTimeOptimalDuration(), 10);
40  BOOST_CHECK_EQUAL(t1.getUserDuration(), 10);
41  //UserDuration lower Than timeOptimal
42  BOOST_CHECK_THROW(t1.setUserDuration(5), InvalidArgumentException);
43 
44  //wrong usage
45  BOOST_CHECK_THROW(t1.setUserDuration(-1), InvalidArgumentException);
46  BOOST_CHECK_THROW(t1.setTimeOptimalDuration(-1), InvalidArgumentException);
47 }
48 
49 BOOST_AUTO_TEST_CASE(deepCopyTest)
50 {
51  //Init
52  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
53  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
54  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
55 
56  Vector3BasePtr pos2 = Vector3BasePtr(new Vector3(4, 5, 6));
57  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(4, 5, 6);
58  PoseBasePtr pose2 = PoseBasePtr(new Pose(pos2, ori2));
59 
62 
63  Vector3BasePtr pos3 = Vector3BasePtr(new Vector3(7, 8, 9));
64  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(7, 8, 9);
65  PoseBasePtr pose3 = PoseBasePtr(new Pose(pos3, ori3));
66 
67  Vector3BasePtr pos4 = Vector3BasePtr(new Vector3(10, 11, 12));
68  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(10, 11, 12);
69  PoseBasePtr pose4 = PoseBasePtr(new Pose(pos4, ori4));
70 
73 
74 
75  Transition t1 = Transition(w1, w2);
77  t1.setUserDuration(20);
78 
79  t1.getStart()->setTimeOptimalTimestamp(1);
80  t1.getStart()->setUserTimestamp(2);
81  t1.getEnd()->setTimeOptimalTimestamp(10);
82  t1.getEnd()->setUserTimestamp(20);
83  Transition t2 = Transition(t1,
85  UserWaypointPtr(new UserWaypoint(*t1.getEnd())));
86 
87 
88  BOOST_CHECK_EQUAL(t2.getTimeOptimalDuration(), 10);
89  BOOST_CHECK_EQUAL(t2.getUserDuration(), 20);
90  BOOST_CHECK_EQUAL(t2.getStart()->getUserTimestamp(), 2);
91  BOOST_CHECK_EQUAL(t2.getStart()->getTimeOptimalTimestamp(), 1);
92  BOOST_CHECK_EQUAL(t2.getEnd()->getUserTimestamp(), 20);
93  BOOST_CHECK_EQUAL(t2.getEnd()->getTimeOptimalTimestamp(), 10);
94 
95 
97  t1.setUserDuration(40);
98  t1.getStart()->setTimeOptimalTimestamp(3);
99  t1.getStart()->setUserTimestamp(4);
100  t1.getEnd()->setTimeOptimalTimestamp(30);
101  t1.getEnd()->setUserTimestamp(40);
102 
103  BOOST_CHECK_EQUAL(t2.getTimeOptimalDuration(), 10);
104  BOOST_CHECK_EQUAL(t2.getUserDuration(), 20);
105  BOOST_CHECK_EQUAL(t2.getStart()->getUserTimestamp(), 2);
106  BOOST_CHECK_EQUAL(t2.getStart()->getTimeOptimalTimestamp(), 1);
107  BOOST_CHECK_EQUAL(t2.getEnd()->getUserTimestamp(), 20);
108  BOOST_CHECK_EQUAL(t2.getEnd()->getTimeOptimalTimestamp(), 10);
109 
110 
111  BOOST_CHECK_EQUAL(t1.getTimeOptimalDuration(), 30);
112  BOOST_CHECK_EQUAL(t1.getUserDuration(), 40);
113  BOOST_CHECK_EQUAL(t1.getStart()->getUserTimestamp(), 4);
114  BOOST_CHECK_EQUAL(t1.getStart()->getTimeOptimalTimestamp(), 3);
115  BOOST_CHECK_EQUAL(t1.getEnd()->getUserTimestamp(), 40);
116  BOOST_CHECK_EQUAL(t1.getEnd()->getTimeOptimalTimestamp(), 30);
117 }
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition: UserWaypoint.h:142
armarx::Transition::setTimeOptimalDuration
void setTimeOptimalDuration(double value)
set the time optimal duration and set the userDuration if its less than time optimal duration
Definition: Transition.cpp:40
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::UserWaypoint
The UserWaypoint class represents a waypoint of the trajectory.
Definition: UserWaypoint.h:47
armarx::Transition::getEnd
UserWaypointPtr getEnd()
Returns the end UserWaypoint of the transition.
Definition: Transition.cpp:102
armarx::Transition::getTimeOptimalDuration
double getTimeOptimalDuration() const
Returns the time optimal duration calculated by TrajectoryCalculation.
Definition: Transition.cpp:34
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:58
armarx::Transition
Definition: Transition.h:36
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: TransitionTests.cpp:12
armarx::eLinearInterpolation
@ eLinearInterpolation
Definition: InterpolationType.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::Transition::getInterpolationType
InterpolationType getInterpolationType() const
Returns the interpolation type of the transition.
Definition: Transition.cpp:59
armarx::Transition::getUserDuration
double getUserDuration() const
Returns the user duration of the transition.
Definition: Transition.cpp:6
armarx::Transition::setUserDuration
void setUserDuration(double value)
Set the user duration of the transition and tests if it is greater than the time optimal duration.
Definition: Transition.cpp:12
armarx::Transition::getStart
UserWaypointPtr getStart()
Returns the start UserWaypoint of the transition.
Definition: Transition.cpp:83