trajectory_unwrap_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Author: Melonee Wise
30  */
31 
32 #include <ros/ros.h>
33 #include <gtest/gtest.h>
34 #include <urdf/model.h>
35 #include <trajectory_msgs/JointTrajectory.h>
36 #include <pr2_arm_ik_action/trajectory_unwrap.h>
37 
38 bool get_model(urdf::Model& robot)
39 {
40  ros::NodeHandle nh;
41  std::string file;
42  nh.getParam("urdf_file_path", file);
43  TiXmlDocument robot_model_xml;
44  robot_model_xml.LoadFile(file);
45  TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
46  if (!robot_xml){
47  std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
48  return false;
49  }
50 
51  if (!robot.initXml(robot_xml)){
52  std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
53  return false;
54  }
55  return true;
56 }
57 
58 
59 TEST(TrajectoryUnwarp, single_unwrap)
60 {
61  double epsilon = 1e-4;
62  urdf::Model robot;
63  EXPECT_TRUE(get_model(robot));
64 
65  trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
66 
67  traj_in.points.resize(2);
68  traj_in.joint_names.push_back("joint1");
69  traj_in.joint_names.push_back("joint2");
70  traj_in.points[0].positions.push_back(0.0);
71  traj_in.points[0].positions.push_back(0.0);
72  traj_in.points[1].positions.push_back(12.0);
73  traj_in.points[1].positions.push_back(0.0);
74 
75 
76  traj_expected =traj_in;
77  traj_expected.points[1].positions[0]=-0.566371;
78  trajectory_unwrap::unwrap(robot, traj_in, traj_out);
79 
80  int points_size = traj_out.points.size();
81  int num_jnts = traj_out.points[0].positions.size();
82 
83  for(int i=0; i<points_size; i++)
84  {
85  for(int j=0; j<num_jnts; j++)
86  {
87  EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
88  }
89  }
90 }
91 
92 TEST(TrajectoryUnwarp, double_unwrap)
93 {
94  double epsilon = 1e-4;
95  urdf::Model robot;
96  EXPECT_TRUE(get_model(robot));
97 
98  trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
99 
100  traj_in.points.resize(2);
101  traj_in.joint_names.push_back("joint1");
102  traj_in.joint_names.push_back("joint3");
103  traj_in.points[0].positions.push_back(0.0);
104  traj_in.points[0].positions.push_back(6.10);
105  traj_in.points[1].positions.push_back(12.0);
106  traj_in.points[1].positions.push_back(12.0);
107 
108  traj_expected =traj_in;
109  traj_expected.points[1].positions[0]=-0.566371;
110  traj_expected.points[1].positions[1]= 5.71682;
111  trajectory_unwrap::unwrap(robot, traj_in, traj_out);
112 
113  int points_size = traj_out.points.size();
114  int num_jnts = traj_out.points[0].positions.size();
115 
116  for(int i=0; i<points_size; i++)
117  {
118  for(int j=0; j<num_jnts; j++)
119  {
120  EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
121  }
122  }
123 }
124 
125 TEST(TrajectoryUnwarp, multipoint_unwrap)
126 {
127  double epsilon = 1e-4;
128  urdf::Model robot;
129  EXPECT_TRUE(get_model(robot));
130 
131  trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
132 
133  traj_in.points.resize(3);
134  traj_in.joint_names.push_back("joint1");
135  traj_in.joint_names.push_back("joint3");
136  traj_in.points[0].positions.push_back(0.0);
137  traj_in.points[0].positions.push_back(6.10);
138  traj_in.points[1].positions.push_back(12.0);
139  traj_in.points[1].positions.push_back(12.0);
140  traj_in.points[2].positions.push_back(1.5);
141  traj_in.points[2].positions.push_back(1.5);
142 
143  traj_expected =traj_in;
144  traj_expected.points[1].positions[0]=-0.566371;
145  traj_expected.points[1].positions[1]= 5.71682;
146  traj_expected.points[2].positions[1]= 7.78318;
147  trajectory_unwrap::unwrap(robot, traj_in, traj_out);
148 
149  int points_size = traj_out.points.size();
150  int num_jnts = traj_out.points[0].positions.size();
151 
152  for(int i=0; i<points_size; i++)
153  {
154  for(int j=0; j<num_jnts; j++)
155  {
156  EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
157  }
158  }
159 }
160 
161 TEST(TrajectoryUnwarp, near_limit_unwrap)
162 {
163  double epsilon = 1e-4;
164  urdf::Model robot;
165  EXPECT_TRUE(get_model(robot));
166 
167  trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
168 
169  traj_in.points.resize(3);
170  traj_in.joint_names.push_back("joint2");
171  traj_in.points[0].positions.push_back(-1.5);
172  traj_in.points[1].positions.push_back(-3.5);
173  traj_in.points[2].positions.push_back(1.7);
174 
175  traj_expected =traj_in;
176  traj_expected.points[1].positions[0]= 2.78318;
177  traj_expected.points[2].positions[0]= 1.7;
178  trajectory_unwrap::unwrap(robot, traj_in, traj_out);
179 
180  int points_size = traj_out.points.size();
181  int num_jnts = traj_out.points[0].positions.size();
182 
183  for(int i=0; i<points_size; i++)
184  {
185  for(int j=0; j<num_jnts; j++)
186  {
187  EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
188  }
189  }
190 }
191 
192 
193 
194 
195 
196 int main(int argc, char **argv){
197  testing::InitGoogleTest(&argc, argv);
198  ros::init(argc, argv, "trajectory_unwrap_test");
199  return RUN_ALL_TESTS();
200 }
trajectory_unwrap::unwrap
void unwrap(urdf::Model &robot_model, const trajectory_msgs::JointTrajectory &traj_in, trajectory_msgs::JointTrajectory &traj_out)
Definition: trajectory_unwrap.h:49
epsilon
double epsilon
ros::NodeHandle
main
int main(int argc, char **argv)
Definition: trajectory_unwrap_test.cpp:196
urdf::Model::initXml
URDF_EXPORT bool initXml(TiXmlElement *xml)
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TEST
TEST(TrajectoryUnwarp, single_unwrap)
Definition: trajectory_unwrap_test.cpp:59
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
get_model
bool get_model(urdf::Model &robot)
Definition: trajectory_unwrap_test.cpp:38

pr2_arm_move_ik
Author(s): Wim Meeusen, Melonee Wise
autogenerated on Fri Jun 7 2019 22:06:41