DesignerTrajectoryTests.cpp
Go to the documentation of this file.
1 #define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectory
2 #define ARMARX_BOOST_TEST
3 
4 #include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
5 #include "../DesignerTrajectory.h"
6 #include "../Util/OrientationConversion.h"
7 #include <VirtualRobot/XML/RobotIO.h>
8 
9 using namespace armarx;
10 
11 namespace
12 {
13  bool equalPose(PoseBasePtr p1, PoseBasePtr p2)
14  {
15  QuaternionPtr q1 = QuaternionPtr::dynamicCast(p1->orientation);
16  QuaternionPtr q2 = QuaternionPtr::dynamicCast(p2->orientation);
17  Vector3Ptr pos1 = Vector3Ptr::dynamicCast(p1->position);
18  Vector3Ptr pos2 = Vector3Ptr::dynamicCast(p2->position);
19 
20  if (q1->toEigen() == q2->toEigen() &&
21  pos1->toEigen() == pos2->toEigen())
22  {
23  return true;
24  }
25  else
26  {
27  return false;
28  }
29  }
30 
31  bool equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
32  {
33  return (w1->getIKSelection() == w2->getIKSelection() &&
34  w1->getIsTimeOptimalBreakpoint() == w2->getIsTimeOptimalBreakpoint() &&
35  w1->getJointAngles() == w2->getJointAngles() &&
36  equalPose(w1->getPose(), w2->getPose()) &&
37  w1->getTimeOptimalTimestamp() == w1->getTimeOptimalTimestamp() &&
38  w1->getUserTimestamp() == w2->getUserTimestamp()) ? true : false ;
39  }
40 
41 
42 }
43 
44 BOOST_AUTO_TEST_CASE(basicTest)
45 {
46  //Init
47  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
48  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
49  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
50 
51  Vector3BasePtr pos2 = Vector3BasePtr(new Vector3(4, 5, 6));
52  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(4, 5, 6);
53  PoseBasePtr pose2 = PoseBasePtr(new Pose(pos2, ori2));
54 
55  Vector3BasePtr pos3 = Vector3BasePtr(new Vector3(7, 8, 9));
56  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(7, 8, 9);
57  PoseBasePtr pose3 = PoseBasePtr(new Pose(pos3, ori3));
58 
59  Vector3BasePtr pos4 = Vector3BasePtr(new Vector3(10, 11, 12));
60  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(10, 11, 12);
61  PoseBasePtr pose4 = PoseBasePtr(new Pose(pos4, ori4));
62 
63  UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
64  w1->setJointAngles({1, 2, 3, 4, 5, 6, 7});
65  UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
66  UserWaypointPtr w3 = UserWaypointPtr(new UserWaypoint(pose3));
67  UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
68 
69  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
70  "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
71 
72  VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
73 
74  w1->setUserTimestamp(10);
75  w2->setUserTimestamp(20);
76  w3->setUserTimestamp(30);
77 
78  DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
79 
80  BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 1);
81 
82  //Wrong usage
83  BOOST_CHECK_THROW(dt1.getUserWaypoint(2), IndexOutOfBoundsException);
84  BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException);
85  BOOST_CHECK_THROW(dt1.getTrajectorySegment(2), IndexOutOfBoundsException);
86 
87  //add UserWaypoints and test Transition
88  dt1.addFirstUserWaypoint(w2);
89  dt1.insertUserWaypoint(w3, 1);
90  dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
91 
92  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
93  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w3);
94  BOOST_CHECK_EQUAL(dt1.getTransition(1)->getStart(), w3);
95  BOOST_CHECK_EQUAL(dt1.getTransition(1)->getEnd(), w1);
96  BOOST_CHECK_EQUAL(dt1.getTransition(2)->getStart(), w1);
97  BOOST_CHECK_EQUAL(dt1.getTransition(2)->getEnd(), w4);
98 
99  //delete UserWaypoint
100  dt1.deleteUserWaypoint(1);
101  BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException); //there should be not three transitions
102  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
103  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w1);
104  BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 3);
105 }
106 
107 BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
108 {
109  //Init Designer Trajectory
110  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
111  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
112  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
113  UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
114  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
115  "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
116  VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
117  DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
118 
119  //Init three Trajectories
120  std::vector<std::vector<double>> data1 =
121  {
122  {1, 2, 3, 4, 5},
123  {1, 2, 3, 4, 5},
124  {1, 2, 3, 4, 5},
125  {1, 2, 3, 4, 5},
126  {1, 2, 3, 4, 5},
127  {1, 2, 3, 4, 5},
128  {1, 2, 3, 4, 5}
129  };
130  std::vector<std::vector<double>> data2 =
131  {
132  {5, 6, 7, 8, 9},
133  {5, 6, 7, 8, 9},
134  {5, 6, 7, 8, 9},
135  {5, 6, 7, 8, 9},
136  {5, 6, 7, 8, 9},
137  {5, 6, 7, 8, 9},
138  {5, 6, 7, 8, 9}
139  };
140  std::vector<std::vector<double>> data3 =
141  {
142  {9, 10, 11, 12, 13},
143  {9, 10, 11, 12, 13},
144  {9, 10, 11, 12, 13},
145  {9, 10, 11, 12, 13},
146  {9, 10, 11, 12, 13},
147  {9, 10, 11, 12, 13},
148  {9, 10, 11, 12, 13}
149  };
150 
151  Ice::DoubleSeq timestamps1 = {0, 1, 2, 3, 4};
152  Ice::DoubleSeq timestamps2 = {0, 1, 2, 3, 4};
153  Ice::DoubleSeq timestamps3 = {0, 1, 2, 3, 4};
154  Ice::StringSeq dimensionNames = {"a", "b", "c", "d", "e", "f", "g"};
155 
156  TrajectoryPtr traj1(new Trajectory(data1, timestamps1, dimensionNames));
157  TrajectoryPtr traj2(new Trajectory(data2, timestamps2, dimensionNames));
158  TrajectoryPtr traj3(new Trajectory(data3, timestamps3, dimensionNames));
159  /////////////////////////////////////////////////////////////////////////////////////
160 
161  dt1.setInterBreakpointTrajectories({traj1, traj2, traj3});
162  BOOST_CHECK_EQUAL(dt1.getInterBreakpointTrajectories().size(), 3);
163 
164 
165  std::vector<std::vector<double>> data4 =
166  {
167  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
168  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
169  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
170  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
171  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
172  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
173  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}
174  };
175 
176  Ice::DoubleSeq timestamps4 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
177  TrajectoryPtr traj4(new Trajectory(data4, timestamps4, dimensionNames));
178 
179  TrajectoryPtr traj5 = dt1.getTimeOptimalTrajectory();
180  // Traj5 should be same as traj4
181  //check every dimension
182  BOOST_CHECK_EQUAL(traj5->getDimensionData(0), traj4->getDimensionData(0));
183  BOOST_CHECK_EQUAL(traj5->getDimensionData(1), traj4->getDimensionData(1));
184  BOOST_CHECK_EQUAL(traj5->getDimensionData(2), traj4->getDimensionData(2));
185  BOOST_CHECK_EQUAL(traj5->getDimensionData(3), traj4->getDimensionData(3));
186  BOOST_CHECK_EQUAL(traj5->getDimensionData(4), traj4->getDimensionData(4));
187  BOOST_CHECK_EQUAL(traj5->getDimensionData(5), traj4->getDimensionData(5));
188  BOOST_CHECK_EQUAL(traj5->getDimensionData(6), traj4->getDimensionData(6));
189  //check timestamps
190  BOOST_CHECK_EQUAL(traj5->getTimestamps(), traj4->getTimestamps());
191 
192  //checkDerivations
193  BOOST_CHECK_EQUAL(traj5->getDerivations(1, 0, 2), traj4->getDerivations(1, 0, 2));
194  BOOST_CHECK_EQUAL(traj5->getDerivations(3, 0, 2), traj4->getDerivations(3, 0, 2));
195  BOOST_CHECK_EQUAL(traj5->getDerivations(5, 0, 2), traj4->getDerivations(5, 0, 2));
196  BOOST_CHECK_EQUAL(traj5->getDerivations(7, 0, 2), traj4->getDerivations(7, 0, 2));
197  BOOST_CHECK_EQUAL(traj5->getDerivations(9, 0, 2), traj4->getDerivations(9, 0, 2));
198  BOOST_CHECK_EQUAL(traj5->getDerivations(11, 0, 2), traj4->getDerivations(11, 0, 2));
199 }
200 
201 BOOST_AUTO_TEST_CASE(getFinalTraj)
202 {
203 }
204 
205 
206 
207 
208 
209 
210 
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition: UserWaypoint.h:137
armarx::DesignerTrajectory::getNrOfUserWaypoints
unsigned int getNrOfUserWaypoints() const
get the number of the userwaypoints
Definition: DesignerTrajectory.cpp:222
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::DesignerTrajectory::addFirstUserWaypoint
void addFirstUserWaypoint(UserWaypointPtr &point)
add a new first userWaypoint
Definition: DesignerTrajectory.cpp:115
armarx::UserWaypoint
The UserWaypoint class represents a waypoint of the trajectory.
Definition: UserWaypoint.h:42
armarx::DesignerTrajectory
Definition: DesignerTrajectory.h:37
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: DesignerTrajectoryTests.cpp:46
armarx::DesignerTrajectory::getInterBreakpointTrajectories
std::vector< TrajectoryPtr > getInterBreakpointTrajectories()
Returns the interBreakpointTrajectories.
Definition: DesignerTrajectory.cpp:24
armarx::DesignerTrajectory::getTimeOptimalTrajectory
TrajectoryPtr getTimeOptimalTrajectory()
get the time optimal trajectory.
Definition: DesignerTrajectory.cpp:253
IceInternal::Handle< Quaternion >
armarx::DesignerTrajectory::deleteUserWaypoint
void deleteUserWaypoint(unsigned int index)
delete the userwaypoint and remove all transitions including the userwaypoint.
Definition: DesignerTrajectory.cpp:190
armarx::DesignerTrajectory::getTransition
TransitionPtr getTransition(unsigned int index)
get the transition
Definition: DesignerTrajectory.cpp:240
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:57
armarx::DesignerTrajectory::getUserWaypoint
UserWaypointPtr getUserWaypoint(unsigned int index)
get the userWaypoint
Definition: DesignerTrajectory.cpp:227
armarx::VariantType::Trajectory
const VariantTypeId Trajectory
Definition: Trajectory.h:44
armarx::DesignerTrajectory::getTrajectorySegment
TrajectoryPtr getTrajectorySegment(unsigned int index)
get one interBreakPoint trajectory
Definition: DesignerTrajectory.cpp:264
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::DesignerTrajectory::addLastUserWaypoint
void addLastUserWaypoint(UserWaypointPtr &point)
add new last userWaypoint
Definition: DesignerTrajectory.cpp:135
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::DesignerTrajectory::setInterBreakpointTrajectories
void setInterBreakpointTrajectories(const std::vector< TrajectoryPtr > &value)
set the interBreakpointTrajectories
Definition: DesignerTrajectory.cpp:29
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::DesignerTrajectory::insertUserWaypoint
void insertUserWaypoint(UserWaypointPtr &point, unsigned int index)
insert userwaypoint before index
Definition: DesignerTrajectory.cpp:153