planning_models: planning_models: test_kinematic

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <planning_models/kinematic_model.h>
00038 #include <planning_models/kinematic_state.h>
00039 #include <gtest/gtest.h>
00040 #include <sstream>
00041 #include <ctype.h>
00042 #include <ros/package.h>
00043 
00044 //urdf location relative to the planning_models path
00045 static const std::string rel_path = "/test_urdf/robot.xml";
00046 
00047 class LoadPlanningModelsPr2 : public testing::Test {
00048 protected:
00049   
00050   virtual void SetUp() {
00051 
00052     full_path_ = ros::package::getPath("planning_models")+rel_path;
00053     
00054     urdf_ok_ = urdf_model_.initFile(full_path_);
00055 
00056   };
00057 
00058   virtual void TearDown() {
00059 
00060   }
00061 
00062 protected:
00063 
00064   urdf::Model urdf_model_;
00065   bool urdf_ok_;
00066   std::string full_path_;
00067 
00068 };
00069 
00070 TEST_F(LoadPlanningModelsPr2, InitOK) 
00071 {
00072   ASSERT_TRUE(urdf_ok_) << full_path_;
00073   ASSERT_EQ(urdf_model_.getName(),"pr2_test");
00074 }
00075 
00076 TEST_F(LoadPlanningModelsPr2, MultidofInit)
00077 {
00078   std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00079 
00080   {
00081     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00082     //with no world multidof we should fail
00083     planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);    
00084     ASSERT_TRUE(kin_model.getRoot() == NULL);
00085   }
00086 
00087   {
00088     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00089     //now this should work with an identity transform
00090     planning_models::KinematicModel::MultiDofConfig config("base_joint");
00091     config.type = "Planar";
00092     config.parent_frame_id = "base_footprint";
00093     config.child_frame_id = "base_footprint";
00094     multi_dof_configs.push_back(config);
00095     
00096     planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);    
00097     ASSERT_TRUE(kin_model.getRoot() != NULL);
00098     EXPECT_EQ(kin_model.getRoot()->getParentFrameId(), "base_footprint");
00099   }
00100 
00101   {
00102     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00103 
00104     //now this should work with an non-identity transform
00105     planning_models::KinematicModel::MultiDofConfig config("world_joint");
00106     config.type = "Floating";
00107     config.parent_frame_id = "odom_combined";
00108     config.child_frame_id = "base_footprint";
00109     multi_dof_configs.push_back(config);
00110     
00111     planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);    
00112     ASSERT_TRUE(kin_model.getRoot() != NULL);
00113     EXPECT_EQ(kin_model.getRoot()->getParentFrameId(), "odom_combined");
00114   }
00115 
00116   //now we test joint state equivalents
00117 }
00118 
00119 TEST_F(LoadPlanningModelsPr2, GroupInit) 
00120 {
00121   std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00122   planning_models::KinematicModel::MultiDofConfig config("base_joint");
00123   config.type = "Planar";
00124   config.parent_frame_id = "base_footprint";
00125   config.child_frame_id = "base_footprint";
00126   multi_dof_configs.push_back(config);
00127 
00128   {
00129     std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00130     //first make sure we can intialize with no groups
00131 
00132     planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);    
00133     ASSERT_TRUE(kin_model.getRoot() != NULL);
00134   }
00135 
00136   {
00137     //if we screw something up we get no group
00138     std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00139     planning_models::KinematicModel::GroupConfig left_arm_base_tip_gc("left_arm_base_tip",
00140                                      "monkey_base",
00141                                      "monkey_tip");
00142     gcs.push_back(left_arm_base_tip_gc);
00143     
00144     std::vector<std::string> left_arm_joints;
00145     left_arm_joints.push_back("l_monkey_pan_joint");
00146     left_arm_joints.push_back("l_monkey_lift_joint");
00147     left_arm_joints.push_back("l_monkey_arm_roll_joint");
00148     left_arm_joints.push_back("l_monkey_flex_joint");
00149     left_arm_joints.push_back("l_monkey_roll_joint");
00150     left_arm_joints.push_back("l_monkey_flex_link");
00151     left_arm_joints.push_back("l_monkey_roll_link");
00152 
00153     std::vector<std::string> subgroups;
00154     
00155     planning_models::KinematicModel::GroupConfig left_arm_joints_gc("left_arm_joints",
00156                                                                     left_arm_joints,
00157                                                                     subgroups);
00158     
00159     gcs.push_back(left_arm_base_tip_gc);
00160     gcs.push_back(left_arm_joints_gc);
00161 
00162     planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);
00163     
00164     const planning_models::KinematicModel::JointModelGroup* left_arm_base_tip_group = kin_model.getModelGroup("left_arm_base_tip");
00165     ASSERT_TRUE(left_arm_base_tip_group == NULL);
00166 
00167     const planning_models::KinematicModel::JointModelGroup* left_arm_joints_group = kin_model.getModelGroup("left_arm_joints");
00168     ASSERT_TRUE(left_arm_joints_group == NULL);
00169   }
00170 
00171   {
00172     std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00173     planning_models::KinematicModel::GroupConfig left_arm_base_tip_gc("left_arm_base_tip",
00174                                                                       "torso_lift_link",
00175                                                                       "l_wrist_roll_link");
00176     
00177     std::vector<std::string> left_arm_joints;
00178     left_arm_joints.push_back("l_shoulder_pan_joint");
00179     left_arm_joints.push_back("l_shoulder_lift_joint");
00180     left_arm_joints.push_back("l_upper_arm_roll_joint");
00181     left_arm_joints.push_back("l_elbow_flex_joint");
00182     left_arm_joints.push_back("l_forearm_roll_joint");
00183     left_arm_joints.push_back("l_wrist_flex_joint");
00184     left_arm_joints.push_back("l_wrist_roll_joint");
00185 
00186     std::vector<std::string> subgroups;
00187     
00188     planning_models::KinematicModel::GroupConfig left_arm_joints_gc("left_arm_joints",
00189                                                                     left_arm_joints,
00190                                                                     subgroups);
00191     
00192     gcs.push_back(left_arm_base_tip_gc);
00193     gcs.push_back(left_arm_joints_gc);
00194 
00195     planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);
00196     
00197     const planning_models::KinematicModel::JointModelGroup* left_arm_base_tip_group = kin_model.getModelGroup("left_arm_base_tip");
00198     ASSERT_TRUE(left_arm_base_tip_group != NULL);
00199 
00200     const planning_models::KinematicModel::JointModelGroup* left_arm_joints_group = kin_model.getModelGroup("left_arm_joints");
00201     ASSERT_TRUE(left_arm_joints_group != NULL);
00202 
00203     for(unsigned int i = 0; i < std::min(left_arm_base_tip_group->getJointModels().size(),
00204                                          left_arm_joints_group->getJointModels().size()); i++) {
00205       EXPECT_EQ(left_arm_base_tip_group->getJointModels()[i]->getName(),
00206                 left_arm_joints_group->getJointModels()[i]->getName());
00207     } 
00208 
00209     EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 7);
00210     EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7);
00211     
00212     bool found_shoulder_pan_link = false;
00213     bool found_wrist_roll_link = false;
00214     for(unsigned int i = 0; i < left_arm_base_tip_group->getGroupLinkModels().size(); i++) {
00215       if(left_arm_base_tip_group->getGroupLinkModels()[i]->getName() == "l_shoulder_pan_link") {
00216         EXPECT_TRUE(found_shoulder_pan_link == false);
00217         found_shoulder_pan_link = true;
00218       }
00219       if(left_arm_base_tip_group->getGroupLinkModels()[i]->getName() == "l_wrist_roll_link") {
00220         EXPECT_TRUE(found_wrist_roll_link == false);
00221         found_wrist_roll_link = true;
00222       }
00223       EXPECT_TRUE(left_arm_base_tip_group->getGroupLinkModels()[i]->getName() != "torso_lift_link");
00224     }
00225     EXPECT_TRUE(found_shoulder_pan_link);
00226     EXPECT_TRUE(found_wrist_roll_link);
00227   }
00228   
00229 }
00230 
00231 int main(int argc, char **argv)
00232 {
00233   testing::InitGoogleTest(&argc, argv);
00234   return RUN_ALL_TESTS();
00235 }
00236
test_kinematic_complicated.cpp
