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
10using namespace armarx;
11
12BOOST_AUTO_TEST_CASE(basicTest)
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
23 UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
24 UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
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);
38 t1.setTimeOptimalDuration(10);
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
49BOOST_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
60 UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
61 UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
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
71 UserWaypointPtr w3 = UserWaypointPtr(new UserWaypoint(pose3));
72 UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
73
74
75 Transition t1 = Transition(w1, w2);
76 t1.setTimeOptimalDuration(10);
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,
84 UserWaypointPtr(new UserWaypoint(*t1.getStart())),
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
96 t1.setTimeOptimalDuration(30);
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}
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition TransitionTests.cpp:12
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition OrientationConversion.cpp:58
armarx::Pose
The Pose class.
Definition Pose.h:243
armarx::Transition::getStart
UserWaypointPtr getStart()
Returns the start UserWaypoint of the transition.
Definition Transition.cpp:83
armarx::Transition::getTimeOptimalDuration
double getTimeOptimalDuration() const
Returns the time optimal duration calculated by TrajectoryCalculation.
Definition Transition.cpp:34
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::getUserDuration
double getUserDuration() const
Returns the user duration of the transition.
Definition Transition.cpp:6
armarx::Transition::getInterpolationType
InterpolationType getInterpolationType() const
Returns the interpolation type of the transition.
Definition Transition.cpp:59
armarx::Transition::getEnd
UserWaypointPtr getEnd()
Returns the end UserWaypoint of the transition.
Definition Transition.cpp:102
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::UserWaypoint
The UserWaypoint class represents a waypoint of the trajectory.
Definition UserWaypoint.h:48
armarx::Vector3
The Vector3 class.
Definition Pose.h:113
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition UserWaypoint.h:142