pr2_arm_move_ik: pr2_arm_move_ik: trajectory_unwrap

00001 /*                                                                                
00002  * Copyright (c) 2008, Willow Garage, Inc.                                        
00003  * All rights reserved.                                                           
00004  *                                                                                
00005  * Redistribution and use in source and binary forms, with or without             
00006  * modification, are permitted provided that the following conditions are met:    
00007  *                                                                                
00008  *     * Redistributions of source code must retain the above copyright           
00009  *       notice, this list of conditions and the following disclaimer.            
00010  *     * Redistributions in binary form must reproduce the above copyright        
00011  *       notice, this list of conditions and the following disclaimer in the      
00012  *       documentation and/or other materials provided with the distribution.     
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its         
00014  *       contributors may be used to endorse or promote products derived from     
00015  *       this software without specific prior written permission.                 
00016  *                                                                                
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"    
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE      
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE     
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE       
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF           
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS       
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN        
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)        
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE     
00027  * POSSIBILITY OF SUCH DAMAGE.                                                    
00028  * 
00029  * Author: Melonee Wise
00030  */
00031 
00032 #include <ros/ros.h>
00033 #include <gtest/gtest.h>
00034 #include <urdf/model.h>
00035 #include <trajectory_msgs/JointTrajectory.h>
00036 #include <pr2_arm_ik_action/trajectory_unwrap.h>
00037 
00038 bool get_model(urdf::Model& robot)
00039 {
00040   ros::NodeHandle nh;
00041   std::string file;
00042   nh.getParam("urdf_file_path", file);
00043   TiXmlDocument robot_model_xml;
00044   robot_model_xml.LoadFile(file);
00045   TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
00046   if (!robot_xml){
00047     std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
00048     return false;
00049   }
00050 
00051   if (!robot.initXml(robot_xml)){
00052     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
00053     return false;
00054   }
00055   return true;
00056 }
00057 
00058 
00059 TEST(TrajectoryUnwarp, single_unwrap)
00060 {
00061   double epsilon = 1e-4;
00062   urdf::Model robot;
00063   EXPECT_TRUE(get_model(robot));
00064 
00065   trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
00066   
00067   traj_in.points.resize(2);
00068   traj_in.joint_names.push_back("joint1");
00069   traj_in.joint_names.push_back("joint2");
00070   traj_in.points[0].positions.push_back(0.0);
00071   traj_in.points[0].positions.push_back(0.0);
00072   traj_in.points[1].positions.push_back(12.0);  
00073   traj_in.points[1].positions.push_back(0.0);
00074 
00075 
00076   traj_expected =traj_in;
00077   traj_expected.points[1].positions[0]=-0.566371;
00078   trajectory_unwrap::unwrap(robot, traj_in, traj_out);
00079  
00080   int points_size = traj_out.points.size();
00081   int num_jnts = traj_out.points[0].positions.size();
00082 
00083   for(int i=0; i<points_size; i++)
00084   {
00085     for(int j=0; j<num_jnts; j++)
00086     {
00087       EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
00088     }
00089   }
00090 }
00091 
00092 TEST(TrajectoryUnwarp, double_unwrap)
00093 {
00094   double epsilon = 1e-4;
00095   urdf::Model robot;
00096   EXPECT_TRUE(get_model(robot));
00097 
00098   trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
00099 
00100   traj_in.points.resize(2);
00101   traj_in.joint_names.push_back("joint1");
00102   traj_in.joint_names.push_back("joint3");
00103   traj_in.points[0].positions.push_back(0.0);
00104   traj_in.points[0].positions.push_back(6.10);
00105   traj_in.points[1].positions.push_back(12.0);
00106   traj_in.points[1].positions.push_back(12.0);
00107 
00108   traj_expected =traj_in;
00109   traj_expected.points[1].positions[0]=-0.566371;
00110   traj_expected.points[1].positions[1]= 5.71682;
00111   trajectory_unwrap::unwrap(robot, traj_in, traj_out);
00112 
00113   int points_size = traj_out.points.size();
00114   int num_jnts = traj_out.points[0].positions.size();
00115 
00116   for(int i=0; i<points_size; i++)
00117   {
00118     for(int j=0; j<num_jnts; j++)
00119     {
00120       EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
00121     }
00122   }
00123 }
00124 
00125 TEST(TrajectoryUnwarp, multipoint_unwrap)
00126 {
00127   double epsilon = 1e-4;
00128   urdf::Model robot;
00129   EXPECT_TRUE(get_model(robot));
00130 
00131   trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
00132 
00133   traj_in.points.resize(3);
00134   traj_in.joint_names.push_back("joint1");
00135   traj_in.joint_names.push_back("joint3");
00136   traj_in.points[0].positions.push_back(0.0);
00137   traj_in.points[0].positions.push_back(6.10);
00138   traj_in.points[1].positions.push_back(12.0);
00139   traj_in.points[1].positions.push_back(12.0);
00140   traj_in.points[2].positions.push_back(1.5);
00141   traj_in.points[2].positions.push_back(1.5);
00142 
00143   traj_expected =traj_in;
00144   traj_expected.points[1].positions[0]=-0.566371;
00145   traj_expected.points[1].positions[1]= 5.71682;
00146   traj_expected.points[2].positions[1]= 7.78318;
00147   trajectory_unwrap::unwrap(robot, traj_in, traj_out);
00148 
00149   int points_size = traj_out.points.size();
00150   int num_jnts = traj_out.points[0].positions.size();
00151 
00152   for(int i=0; i<points_size; i++)
00153   {
00154     for(int j=0; j<num_jnts; j++)
00155     {
00156       EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
00157     }
00158   }
00159 }
00160 
00161 TEST(TrajectoryUnwarp, near_limit_unwrap)
00162 {
00163   double epsilon = 1e-4;
00164   urdf::Model robot;
00165   EXPECT_TRUE(get_model(robot));
00166 
00167   trajectory_msgs::JointTrajectory traj_in, traj_out, traj_expected;
00168 
00169   traj_in.points.resize(3);
00170   traj_in.joint_names.push_back("joint2");
00171   traj_in.points[0].positions.push_back(-1.5);
00172   traj_in.points[1].positions.push_back(-3.5);
00173   traj_in.points[2].positions.push_back(1.7);
00174 
00175   traj_expected =traj_in;
00176   traj_expected.points[1].positions[0]= 2.78318;
00177   traj_expected.points[2].positions[0]= 1.7;
00178   trajectory_unwrap::unwrap(robot, traj_in, traj_out);
00179 
00180   int points_size = traj_out.points.size();
00181   int num_jnts = traj_out.points[0].positions.size();
00182 
00183   for(int i=0; i<points_size; i++)
00184   {
00185     for(int j=0; j<num_jnts; j++)
00186     {
00187       EXPECT_NEAR(traj_expected.points[i].positions[j],traj_out.points[i].positions[j], epsilon);
00188     }
00189   }
00190 }
00191 
00192 
00193 
00194 
00195 
00196 int main(int argc, char **argv){
00197   testing::InitGoogleTest(&argc, argv);
00198   ros::init(argc, argv, "trajectory_unwrap_test");
00199   return RUN_ALL_TESTS();
00200 }
trajectory_unwrap_test.cpp
