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