Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
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
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
00131
00132 planning_models::KinematicModel kin_model(urdf_model_,gcs, multi_dof_configs);
00133 ASSERT_TRUE(kin_model.getRoot() != NULL);
00134 }
00135
00136 {
00137
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