cartesian_trajectory_test.cpp
Go to the documentation of this file.
1 // -- BEGIN LICENSE BLOCK ----------------------------------------------
2 // Copyright 2021 FZI Forschungszentrum Informatik
3 // Created on behalf of Universal Robots A/S
4 //
5 // Licensed under the Apache License, Version 2.0 (the "License");
6 // you may not use this file except in compliance with the License.
7 // You may obtain a copy of the License at
8 //
9 // http://www.apache.org/licenses/LICENSE-2.0
10 //
11 // Unless required by applicable law or agreed to in writing, software
12 // distributed under the License is distributed on an "AS IS" BASIS,
13 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 // See the License for the specific language governing permissions and
15 // limitations under the License.
16 // -- END LICENSE BLOCK ------------------------------------------------
17 
18 //-----------------------------------------------------------------------------
25 //-----------------------------------------------------------------------------
26 
27 #include <gtest/gtest.h>
28 
29 #include <cartesian_trajectory_interpolation/cartesian_trajectory.h>
30 #include <cartesian_trajectory_interpolation/cartesian_state.h>
31 #include <cartesian_control_msgs/CartesianTrajectory.h>
32 #include "Eigen/src/Core/Matrix.h"
33 #include "cartesian_control_msgs/CartesianTrajectoryPoint.h"
34 #include "ros/duration.h"
35 #include <ros/ros.h>
36 #include <ostream>
37 
38 using namespace ros_controllers_cartesian;
39 
40 TEST(TestCartesianTrajectory, SamplingFromEmptyTrajectoryGivesEmptyState)
41 {
42  CartesianState a;
43  CartesianState b;
44 
45  CartesianTrajectory empty_traj;
46  empty_traj.sample(-0.5, a);
47 
48  std::stringstream a_str;
49  std::stringstream b_str;
50  a_str << a;
51  b_str << b;
52 
53  EXPECT_EQ(b_str.str(), a_str.str());
54 }
55 
56 TEST(TestCartesianTrajectory, DefaultQuaternionsGiveValidInterpolation)
57 {
58  auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint();
59  p1.pose.position.x = 0.0;
60  p1.pose.position.y = 0.0;
61  p1.pose.position.z = 0.0;
62  p1.time_from_start = ros::Duration(0.0);
63 
64  auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint();
65  p2.pose.position.x = 1.0;
66  p2.pose.position.y = 1.0;
67  p2.pose.position.z = 1.0;
68  p2.time_from_start = ros::Duration(5.0);
69 
70  cartesian_control_msgs::CartesianTrajectory traj;
71  traj.points.push_back(p1);
72  traj.points.push_back(p2);
73 
74  auto c_traj = CartesianTrajectory(traj);
75  CartesianState c;
76  c_traj.sample(2.5, c);
77 
78  std::stringstream result;
79  result << c;
80 
81  // Check for nan values as a frequent result for invalid, all-zero quaternion operations.
82  EXPECT_EQ(std::string::npos, result.str().find("nan"));
83 }
84 
85 TEST(TestCartesianTrajectory, NonIncreasingWaypointsInTimeFail)
86 {
87  auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint();
88  auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint();
89  p1.time_from_start = ros::Duration(1.0);
90  p2.time_from_start = ros::Duration(0.9);
91 
92  cartesian_control_msgs::CartesianTrajectory init;
93  init.points.push_back(p1);
94  init.points.push_back(p2);
95 
96  CartesianTrajectory c_traj;
97  EXPECT_FALSE(c_traj.init(init));
98 }
99 
100 TEST(TestCartesianTrajectory, InterpolationGivesPlausibleResults)
101 {
102  auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint();
103  p1.pose.position.x = 0.0;
104  p1.pose.position.y = 0.0;
105  p1.pose.position.z = 0.0;
106  p1.pose.orientation.x = 0;
107  p1.pose.orientation.y = 0;
108  p1.pose.orientation.z = 0;
109  p1.pose.orientation.w = 1;
110  p1.time_from_start = ros::Duration(0.0);
111 
112  auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint();
113  p2.pose.position.x = 1.1;
114  p2.pose.position.y = 2.2;
115  p2.pose.position.z = 3.3;
116  p2.pose.orientation.x = 1; // 180 degrees around x
117  p2.pose.orientation.y = 0;
118  p2.pose.orientation.z = 0;
119  p2.pose.orientation.w = 0;
120  p2.time_from_start = ros::Duration(10.0);
121 
122  cartesian_control_msgs::CartesianTrajectory traj;
123  traj.points.push_back(p1);
124  traj.points.push_back(p2);
125 
126  auto c_traj = CartesianTrajectory(traj);
127  CartesianState c;
128  c_traj.sample(5, c);
129 
130  constexpr double pi = 3.14159265358979323846;
131 
132  // For the linear case, half the time should give half the values.
133  EXPECT_DOUBLE_EQ(p2.pose.position.x / 2.0, c.p.x());
134  EXPECT_DOUBLE_EQ(p2.pose.position.y / 2.0, c.p.y());
135  EXPECT_DOUBLE_EQ(p2.pose.position.z / 2.0, c.p.z());
136  EXPECT_DOUBLE_EQ(pi / 2.0, c.rot().norm());
137 }
138 
139 TEST(TestCartesianTrajectory, OppositeSignsGivesSameResults)
140 {
141  // Since opposite sign is the same orientation, it shouldn't interpolate the orientation in this case
142  auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint();
143  p1.pose.position.x = 0.0;
144  p1.pose.position.y = 0.0;
145  p1.pose.position.z = 0.0;
146  p1.pose.orientation.x = 0.0353693;
147  p1.pose.orientation.y = 0.984552;
148  p1.pose.orientation.z = 0.1707371;
149  p1.pose.orientation.w = 0.0159721;
150  p1.time_from_start = ros::Duration(0.0);
151 
152  auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint();
153  p2.pose.position.x = 0.0;
154  p2.pose.position.y = 0.0;
155  p2.pose.position.z = 0.0;
156  // Sign flip
157  p2.pose.orientation.x = -p1.pose.orientation.x;
158  p2.pose.orientation.y = -p1.pose.orientation.y;
159  p2.pose.orientation.z = -p1.pose.orientation.z;
160  p2.pose.orientation.w = -p1.pose.orientation.w;
161  p2.time_from_start = ros::Duration(5.0);
162 
163  cartesian_control_msgs::CartesianTrajectory traj;
164  traj.points.push_back(p1);
165  traj.points.push_back(p2);
166 
167  auto c_traj = CartesianTrajectory(traj);
168  CartesianState c1, c2;
169  c_traj.sample(1.0, c1);
170  c_traj.sample(4.0, c2);
171 
172  EXPECT_DOUBLE_EQ(c1.q.x(), c2.q.x());
173  EXPECT_DOUBLE_EQ(c1.q.y(), c2.q.y());
174  EXPECT_DOUBLE_EQ(c1.q.z(), c2.q.z());
175  EXPECT_DOUBLE_EQ(c1.q.w(), c2.q.w());
176 }
177 
178 TEST(TestCartesianTrajectory, ShortestPathIsFollowed)
179 {
180  auto p1 = cartesian_control_msgs::CartesianTrajectoryPoint();
181  p1.pose.position.x = 0.0;
182  p1.pose.position.y = 0.0;
183  p1.pose.position.z = 0.0;
184  p1.pose.orientation.x = 0.1046779;
185  p1.pose.orientation.y = -0.7045626;
186  p1.pose.orientation.z = 0.1234662;
187  p1.pose.orientation.w = 0.6909343;
188  p1.time_from_start = ros::Duration(0.0);
189 
190  auto p2 = cartesian_control_msgs::CartesianTrajectoryPoint();
191  p2.pose.position.x = 0.0;
192  p2.pose.position.y = 0.0;
193  p2.pose.position.z = 0.0;
194  // Sign flip
195  p2.pose.orientation.x = 0.0485966;
196  p2.pose.orientation.y = 0.7107247;
197  p2.pose.orientation.z = 0.031284;
198  p2.pose.orientation.w = -0.7010921;
199  p2.time_from_start = ros::Duration(5.0);
200 
201  cartesian_control_msgs::CartesianTrajectory traj;
202  traj.points.push_back(p1);
203  traj.points.push_back(p2);
204 
205  auto c_traj = CartesianTrajectory(traj);
206  CartesianState c;
207  c_traj.sample(5.0, c);
208 
209  // To follow the shorets path, the second trajectory point should be inverted
210  EXPECT_NEAR(p2.pose.orientation.x * -1, c.q.x(), 0.001);
211  EXPECT_NEAR(p2.pose.orientation.y * -1, c.q.y(), 0.001);
212  EXPECT_NEAR(p2.pose.orientation.z * -1, c.q.z(), 0.001);
213  EXPECT_NEAR(p2.pose.orientation.w * -1, c.q.w(), 0.001);
214 }
215 
216 int main(int argc, char** argv)
217 {
218  testing::InitGoogleTest(&argc, argv);
219  return RUN_ALL_TESTS();
220 }
ros_controllers_cartesian
Definition: cartesian_state.h:29
ros.h
ros_controllers_cartesian::CartesianTrajectory::init
bool init(const cartesian_control_msgs::CartesianTrajectory &ros_trajectory)
Initialize from ROS message.
Definition: cartesian_trajectory.cpp:45
main
int main(int argc, char **argv)
Definition: cartesian_trajectory_test.cpp:216
ros_controllers_cartesian::CartesianState::q
Eigen::Quaterniond q
rotation
Definition: cartesian_state.h:98
duration.h
ros_controllers_cartesian::CartesianState::rot
Eigen::Vector3d rot() const
Get Euler-Rodrigues vector from orientation.
Definition: cartesian_state.h:80
ros_controllers_cartesian::CartesianState
Cartesian state with pose, velocity and acceleration.
Definition: cartesian_state.h:38
cartesian_state.h
ros_controllers_cartesian::CartesianTrajectory
A class for Cartesian trajectory representation and interpolation.
Definition: cartesian_trajectory.h:45
cartesian_trajectory.h
ros_controllers_cartesian::CartesianState::p
Eigen::Vector3d p
position
Definition: cartesian_state.h:97
ros_controllers_cartesian::CartesianTrajectory::sample
void sample(const CartesianTrajectorySegment::Time &time, CartesianState &state)
Sample a trajectory at a specified time.
Definition: cartesian_trajectory.cpp:32
init
void init(const M_string &remappings)
ros::Duration
TEST
TEST(TestCartesianTrajectory, SamplingFromEmptyTrajectoryGivesEmptyState)
Definition: cartesian_trajectory_test.cpp:40

cartesian_trajectory_interpolation
Author(s):
autogenerated on Tue Oct 15 2024 02:09:14