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 <RobotComponents/Test.h>
5 
6 #include "../DesignerTrajectory.h"
7 #include "../Util/OrientationConversion.h"
8 #include <VirtualRobot/XML/RobotIO.h>
9 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
10 
11 using namespace armarx;
12 
13 namespace
14 {
15  bool equalPose(PoseBasePtr p1, PoseBasePtr p2)
16  {
17  QuaternionPtr q1 = QuaternionPtr::dynamicCast(p1->orientation);
18  QuaternionPtr q2 = QuaternionPtr::dynamicCast(p2->orientation);
19  Vector3Ptr pos1 = Vector3Ptr::dynamicCast(p1->position);
20  Vector3Ptr pos2 = Vector3Ptr::dynamicCast(p2->position);
21 
22  if (q1->toEigen() == q2->toEigen() &&
23  pos1->toEigen() == pos2->toEigen())
24  {
25  return true;
26  }
27  else
28  {
29  return false;
30  }
31  }
32 
33  bool equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
34  {
35  return (w1->getIKSelection() == w2->getIKSelection() &&
36  w1->getIsTimeOptimalBreakpoint() == w2->getIsTimeOptimalBreakpoint() &&
37  w1->getJointAngles() == w2->getJointAngles() &&
38  equalPose(w1->getPose(), w2->getPose()) &&
39  w1->getTimeOptimalTimestamp() == w1->getTimeOptimalTimestamp() &&
40  w1->getUserTimestamp() == w2->getUserTimestamp()) ? true : false ;
41  }
42 
43 
44 }
45 
46 BOOST_AUTO_TEST_CASE(basicTest)
47 {
48  //Init
49  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
50  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
51  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
52 
53  Vector3BasePtr pos2 = Vector3BasePtr(new Vector3(4, 5, 6));
54  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(4, 5, 6);
55  PoseBasePtr pose2 = PoseBasePtr(new Pose(pos2, ori2));
56 
57  Vector3BasePtr pos3 = Vector3BasePtr(new Vector3(7, 8, 9));
58  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(7, 8, 9);
59  PoseBasePtr pose3 = PoseBasePtr(new Pose(pos3, ori3));
60 
61  Vector3BasePtr pos4 = Vector3BasePtr(new Vector3(10, 11, 12));
62  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(10, 11, 12);
63  PoseBasePtr pose4 = PoseBasePtr(new Pose(pos4, ori4));
64 
65  UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
66  w1->setJointAngles({1, 2, 3, 4, 5, 6, 7});
67  UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
68  UserWaypointPtr w3 = UserWaypointPtr(new UserWaypoint(pose3));
69  UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
70 
71  VirtualRobot::RobotPtr robot;
72  std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
73  CMakePackageFinder finder("RobotAPI");
74 
75  if (finder.packageFound())
76  {
77  robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
78  }
79 
80  VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
81 
82  w1->setUserTimestamp(10);
83  w2->setUserTimestamp(20);
84  w3->setUserTimestamp(30);
85 
86  DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
87 
88  BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 1);
89 
90  //Wrong usage
91  BOOST_CHECK_THROW(dt1.getUserWaypoint(2), IndexOutOfBoundsException);
92  BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException);
93  BOOST_CHECK_THROW(dt1.getTrajectorySegment(2), IndexOutOfBoundsException);
94 
95  //add UserWaypoints and test Transition
96  dt1.addFirstUserWaypoint(w2);
97  dt1.insertUserWaypoint(w3, 1);
98  dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
99 
100  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
101  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w3);
102  BOOST_CHECK_EQUAL(dt1.getTransition(1)->getStart(), w3);
103  BOOST_CHECK_EQUAL(dt1.getTransition(1)->getEnd(), w1);
104  BOOST_CHECK_EQUAL(dt1.getTransition(2)->getStart(), w1);
105  BOOST_CHECK_EQUAL(dt1.getTransition(2)->getEnd(), w4);
106 
107  //delete UserWaypoint
108  dt1.deleteUserWaypoint(1);
109  BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException); //there should be not three transitions
110  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
111  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w1);
112  BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 3);
113 }
114 
115 BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
116 {
117  //Init Designer Trajectory
118  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
119  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
120  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
121  UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
122  w1->setJointAngles({1, 2, 3, 4, 5, 6, 7});
123 
124  // Get Robot
125  std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
126  CMakePackageFinder finder("RobotAPI");
127  VirtualRobot::RobotPtr robot;
128  if (finder.packageFound())
129  {
130  robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
131  }
132  else
133  {
134  robot = VirtualRobot::RobotIO::loadRobot(
135  "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
136  }
137  VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
138  DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
139 
140  //Init three Trajectories
141  std::vector<std::vector<double>> data1 =
142  {
143  {1, 2, 3, 4, 5},
144  {1, 2, 3, 4, 5},
145  {1, 2, 3, 4, 5},
146  {1, 2, 3, 4, 5},
147  {1, 2, 3, 4, 5},
148  {1, 2, 3, 4, 5},
149  {1, 2, 3, 4, 5}
150  };
151  std::vector<std::vector<double>> data2 =
152  {
153  {5, 6, 7, 8, 9},
154  {5, 6, 7, 8, 9},
155  {5, 6, 7, 8, 9},
156  {5, 6, 7, 8, 9},
157  {5, 6, 7, 8, 9},
158  {5, 6, 7, 8, 9},
159  {5, 6, 7, 8, 9}
160  };
161  std::vector<std::vector<double>> data3 =
162  {
163  {9, 10, 11, 12, 13},
164  {9, 10, 11, 12, 13},
165  {9, 10, 11, 12, 13},
166  {9, 10, 11, 12, 13},
167  {9, 10, 11, 12, 13},
168  {9, 10, 11, 12, 13},
169  {9, 10, 11, 12, 13}
170  };
171 
172  Ice::DoubleSeq timestamps1 = {0, 1, 2, 3, 4};
173  Ice::DoubleSeq timestamps2 = {0, 1, 2, 3, 4};
174  Ice::DoubleSeq timestamps3 = {0, 1, 2, 3, 4};
175  Ice::StringSeq dimensionNames = {"a", "b", "c", "d", "e", "f", "g"};
176 
177  TrajectoryPtr traj1(new Trajectory(data1, timestamps1, dimensionNames));
178  TrajectoryPtr traj2(new Trajectory(data2, timestamps2, dimensionNames));
179  TrajectoryPtr traj3(new Trajectory(data3, timestamps3, dimensionNames));
180  /////////////////////////////////////////////////////////////////////////////////////
181 
182  dt1.setInterBreakpointTrajectories({traj1, traj2, traj3});
183  BOOST_CHECK_EQUAL(dt1.getInterBreakpointTrajectories().size(), 3);
184 
185 
186  std::vector<std::vector<double>> data4 =
187  {
188  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
189  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
190  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
191  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
192  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
193  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
194  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}
195  };
196 
197  Ice::DoubleSeq timestamps4 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
198  TrajectoryPtr traj4(new Trajectory(data4, timestamps4, dimensionNames));
199 
200  TrajectoryPtr traj5 = dt1.getTimeOptimalTrajectory();
201  // Traj5 should be same as traj4
202  //check every dimension
203  BOOST_CHECK_EQUAL(traj5->getDimensionData(0), traj4->getDimensionData(0));
204  BOOST_CHECK_EQUAL(traj5->getDimensionData(1), traj4->getDimensionData(1));
205  BOOST_CHECK_EQUAL(traj5->getDimensionData(2), traj4->getDimensionData(2));
206  BOOST_CHECK_EQUAL(traj5->getDimensionData(3), traj4->getDimensionData(3));
207  BOOST_CHECK_EQUAL(traj5->getDimensionData(4), traj4->getDimensionData(4));
208  BOOST_CHECK_EQUAL(traj5->getDimensionData(5), traj4->getDimensionData(5));
209  BOOST_CHECK_EQUAL(traj5->getDimensionData(6), traj4->getDimensionData(6));
210  //check timestamps
211  BOOST_CHECK_EQUAL(traj5->getTimestamps(), traj4->getTimestamps());
212 
213  //checkDerivations
214  BOOST_CHECK_EQUAL(traj5->getDerivations(1, 0, 2), traj4->getDerivations(1, 0, 2));
215  BOOST_CHECK_EQUAL(traj5->getDerivations(3, 0, 2), traj4->getDerivations(3, 0, 2));
216  BOOST_CHECK_EQUAL(traj5->getDerivations(5, 0, 2), traj4->getDerivations(5, 0, 2));
217  BOOST_CHECK_EQUAL(traj5->getDerivations(7, 0, 2), traj4->getDerivations(7, 0, 2));
218  BOOST_CHECK_EQUAL(traj5->getDerivations(9, 0, 2), traj4->getDerivations(9, 0, 2));
219  BOOST_CHECK_EQUAL(traj5->getDerivations(11, 0, 2), traj4->getDerivations(11, 0, 2));
220 }
221 
222 BOOST_AUTO_TEST_CASE(getFinalTraj)
223 {
224 }
225 
226 
227 
228 
229 
230 
231 
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::CMakePackageFinder::packageFound
bool packageFound() const
Returns whether or not this package was found with cmake.
Definition: CMakePackageFinder.cpp:485
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::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:53
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::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:176
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
CMakePackageFinder.h
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