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 "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
7 #include "../DesignerTrajectory.h"
8 #include "../Util/OrientationConversion.h"
9 
10 using namespace armarx;
11 
12 namespace
13 {
14  bool
15  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() && pos1->toEigen() == pos2->toEigen())
23  {
24  return true;
25  }
26  else
27  {
28  return false;
29  }
30  }
31 
32  bool
33  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())
41  ? true
42  : false;
43  }
44 
45 
46 } // namespace
47 
48 BOOST_AUTO_TEST_CASE(basicTest)
49 {
50  //Init
51  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
52  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
53  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
54 
55  Vector3BasePtr pos2 = Vector3BasePtr(new Vector3(4, 5, 6));
56  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(4, 5, 6);
57  PoseBasePtr pose2 = PoseBasePtr(new Pose(pos2, ori2));
58 
59  Vector3BasePtr pos3 = Vector3BasePtr(new Vector3(7, 8, 9));
60  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(7, 8, 9);
61  PoseBasePtr pose3 = PoseBasePtr(new Pose(pos3, ori3));
62 
63  Vector3BasePtr pos4 = Vector3BasePtr(new Vector3(10, 11, 12));
64  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(10, 11, 12);
65  PoseBasePtr pose4 = PoseBasePtr(new Pose(pos4, ori4));
66 
67  UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
68  w1->setJointAngles({1, 2, 3, 4, 5, 6, 7});
69  UserWaypointPtr w2 = UserWaypointPtr(new UserWaypoint(pose2));
70  UserWaypointPtr w3 = UserWaypointPtr(new UserWaypoint(pose3));
71  UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
72 
73  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
74  "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
75 
76  VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
77 
78  w1->setUserTimestamp(10);
79  w2->setUserTimestamp(20);
80  w3->setUserTimestamp(30);
81 
82  DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
83 
84  BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 1);
85 
86  //Wrong usage
87  BOOST_CHECK_THROW(dt1.getUserWaypoint(2), IndexOutOfBoundsException);
88  BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException);
89  BOOST_CHECK_THROW(dt1.getTrajectorySegment(2), IndexOutOfBoundsException);
90 
91  //add UserWaypoints and test Transition
92  dt1.addFirstUserWaypoint(w2);
93  dt1.insertUserWaypoint(w3, 1);
94  dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
95 
96  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
97  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w3);
98  BOOST_CHECK_EQUAL(dt1.getTransition(1)->getStart(), w3);
99  BOOST_CHECK_EQUAL(dt1.getTransition(1)->getEnd(), w1);
100  BOOST_CHECK_EQUAL(dt1.getTransition(2)->getStart(), w1);
101  BOOST_CHECK_EQUAL(dt1.getTransition(2)->getEnd(), w4);
102 
103  //delete UserWaypoint
104  dt1.deleteUserWaypoint(1);
105  BOOST_CHECK_THROW(dt1.getTransition(2),
106  IndexOutOfBoundsException); //there should be not three transitions
107  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
108  BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w1);
109  BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 3);
110 }
111 
112 BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
113 {
114  //Init Designer Trajectory
115  Vector3BasePtr pos1 = Vector3BasePtr(new Vector3(1, 2, 3));
116  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 2, 3);
117  PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
118  UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
119  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
120  "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
121  VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
122  DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
123 
124  //Init three Trajectories
125  std::vector<std::vector<double>> data1 = {{1, 2, 3, 4, 5},
126  {1, 2, 3, 4, 5},
127  {1, 2, 3, 4, 5},
128  {1, 2, 3, 4, 5},
129  {1, 2, 3, 4, 5},
130  {1, 2, 3, 4, 5},
131  {1, 2, 3, 4, 5}};
132  std::vector<std::vector<double>> data2 = {{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  std::vector<std::vector<double>> data3 = {{9, 10, 11, 12, 13},
140  {9, 10, 11, 12, 13},
141  {9, 10, 11, 12, 13},
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 
147  Ice::DoubleSeq timestamps1 = {0, 1, 2, 3, 4};
148  Ice::DoubleSeq timestamps2 = {0, 1, 2, 3, 4};
149  Ice::DoubleSeq timestamps3 = {0, 1, 2, 3, 4};
150  Ice::StringSeq dimensionNames = {"a", "b", "c", "d", "e", "f", "g"};
151 
152  TrajectoryPtr traj1(new Trajectory(data1, timestamps1, dimensionNames));
153  TrajectoryPtr traj2(new Trajectory(data2, timestamps2, dimensionNames));
154  TrajectoryPtr traj3(new Trajectory(data3, timestamps3, dimensionNames));
155  /////////////////////////////////////////////////////////////////////////////////////
156 
157  dt1.setInterBreakpointTrajectories({traj1, traj2, traj3});
158  BOOST_CHECK_EQUAL(dt1.getInterBreakpointTrajectories().size(), 3);
159 
160 
161  std::vector<std::vector<double>> data4 = {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
162  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
163  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
164  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
165  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
166  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
167  {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}};
168 
169  Ice::DoubleSeq timestamps4 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
170  TrajectoryPtr traj4(new Trajectory(data4, timestamps4, dimensionNames));
171 
172  TrajectoryPtr traj5 = dt1.getTimeOptimalTrajectory();
173  // Traj5 should be same as traj4
174  //check every dimension
175  BOOST_CHECK_EQUAL(traj5->getDimensionData(0), traj4->getDimensionData(0));
176  BOOST_CHECK_EQUAL(traj5->getDimensionData(1), traj4->getDimensionData(1));
177  BOOST_CHECK_EQUAL(traj5->getDimensionData(2), traj4->getDimensionData(2));
178  BOOST_CHECK_EQUAL(traj5->getDimensionData(3), traj4->getDimensionData(3));
179  BOOST_CHECK_EQUAL(traj5->getDimensionData(4), traj4->getDimensionData(4));
180  BOOST_CHECK_EQUAL(traj5->getDimensionData(5), traj4->getDimensionData(5));
181  BOOST_CHECK_EQUAL(traj5->getDimensionData(6), traj4->getDimensionData(6));
182  //check timestamps
183  BOOST_CHECK_EQUAL(traj5->getTimestamps(), traj4->getTimestamps());
184 
185  //checkDerivations
186  BOOST_CHECK_EQUAL(traj5->getDerivations(1, 0, 2), traj4->getDerivations(1, 0, 2));
187  BOOST_CHECK_EQUAL(traj5->getDerivations(3, 0, 2), traj4->getDerivations(3, 0, 2));
188  BOOST_CHECK_EQUAL(traj5->getDerivations(5, 0, 2), traj4->getDerivations(5, 0, 2));
189  BOOST_CHECK_EQUAL(traj5->getDerivations(7, 0, 2), traj4->getDerivations(7, 0, 2));
190  BOOST_CHECK_EQUAL(traj5->getDerivations(9, 0, 2), traj4->getDerivations(9, 0, 2));
191  BOOST_CHECK_EQUAL(traj5->getDerivations(11, 0, 2), traj4->getDerivations(11, 0, 2));
192 }
193 
194 BOOST_AUTO_TEST_CASE(getFinalTraj)
195 {
196 }
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::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::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::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
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