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 <moveit/robot_model/robot_model.h>
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/robot_state/conversions.h>
00040 #include <urdf_parser/urdf_parser.h>
00041 #include <fstream>
00042 #include <gtest/gtest.h>
00043
00044 class LoadPlanningModelsPr2 : public testing::Test
00045 {
00046 protected:
00047
00048 virtual void SetUp()
00049 {
00050 srdf_model_.reset(new srdf::Model());
00051
00052 std::string xml_string;
00053 std::fstream xml_file("test/urdf/robot.xml", std::fstream::in);
00054 if (xml_file.is_open())
00055 {
00056 while ( xml_file.good() )
00057 {
00058 std::string line;
00059 std::getline( xml_file, line);
00060 xml_string += (line + "\n");
00061 }
00062 xml_file.close();
00063 urdf_model_ = urdf::parseURDF(xml_string);
00064 urdf_ok_ = urdf_model_;
00065 }
00066 else
00067 urdf_ok_ = false;
00068 srdf_ok_ = srdf_model_->initFile(*urdf_model_, "test/srdf/robot.xml");
00069 };
00070
00071 virtual void TearDown()
00072 {
00073 }
00074
00075 protected:
00076
00077 boost::shared_ptr<urdf::ModelInterface> urdf_model_;
00078 boost::shared_ptr<srdf::Model> srdf_model_;
00079 bool urdf_ok_;
00080 bool srdf_ok_;
00081
00082 };
00083
00084 TEST_F(LoadPlanningModelsPr2, InitOK)
00085 {
00086 ASSERT_TRUE(urdf_ok_);
00087 ASSERT_TRUE(srdf_ok_);
00088 ASSERT_EQ(urdf_model_->getName(), "pr2_test");
00089 ASSERT_EQ(srdf_model_->getName(), "pr2_test");
00090 }
00091
00092 TEST_F(LoadPlanningModelsPr2, MultidofInit)
00093 {
00094 boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
00095
00096
00097 robot_model::RobotModel kin_model0(urdf_model_, srdfModel);
00098 EXPECT_TRUE(kin_model0.getRoot()->getVariableCount() == 0);
00099
00100 static const std::string SMODEL1 =
00101 "<?xml version=\"1.0\" ?>"
00102 "<robot name=\"pr2_test\">"
00103 "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>"
00104 "</robot>";
00105 srdfModel->initString(*urdf_model_, SMODEL1);
00106
00107 robot_model::RobotModel kin_model1(urdf_model_, srdfModel);
00108 ASSERT_TRUE(kin_model1.getRoot() != NULL);
00109 EXPECT_EQ(kin_model1.getModelFrame(), "base_footprint");
00110
00111 static const std::string SMODEL2 =
00112 "<?xml version=\"1.0\" ?>"
00113 "<robot name=\"pr2_test\">"
00114 "<virtual_joint name=\"world_joint\" child_link=\"base_footprint\" parent_frame=\"odom_combined\" type=\"floating\"/>"
00115 "</robot>";
00116 srdfModel->initString(*urdf_model_, SMODEL2);
00117
00118 robot_model::RobotModel kin_model2(urdf_model_, srdfModel);
00119 ASSERT_TRUE(kin_model2.getRoot() != NULL);
00120 EXPECT_EQ(kin_model2.getModelFrame(), "odom_combined");
00121 }
00122
00123 TEST_F(LoadPlanningModelsPr2, GroupInit)
00124 {
00125 static const std::string SMODEL1 =
00126 "<?xml version=\"1.0\" ?>"
00127 "<robot name=\"pr2_test\">"
00128 "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>"
00129 "<group name=\"left_arm_base_tip\">"
00130 "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>"
00131 "</group>"
00132 "<group name=\"left_arm_joints\">"
00133 "<joint name=\"l_monkey_pan_joint\"/>"
00134 "<joint name=\"l_monkey_fles_joint\"/>"
00135 "</group>"
00136 "</robot>";
00137
00138 boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
00139 srdfModel->initString(*urdf_model_, SMODEL1);
00140 robot_model::RobotModel kin_model1(urdf_model_, srdfModel);
00141
00142 const robot_model::JointModelGroup* left_arm_base_tip_group = kin_model1.getJointModelGroup("left_arm_base_tip");
00143 ASSERT_TRUE(left_arm_base_tip_group == NULL);
00144
00145 const robot_model::JointModelGroup* left_arm_joints_group = kin_model1.getJointModelGroup("left_arm_joints");
00146 ASSERT_TRUE(left_arm_joints_group == NULL);
00147
00148 static const std::string SMODEL2 =
00149 "<?xml version=\"1.0\" ?>"
00150 "<robot name=\"pr2_test\">"
00151 "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>"
00152 "<group name=\"left_arm_base_tip\">"
00153 "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>"
00154 "</group>"
00155 "<group name=\"left_arm_joints\">"
00156 "<joint name=\"l_shoulder_pan_joint\"/>"
00157 "<joint name=\"l_shoulder_lift_joint\"/>"
00158 "<joint name=\"l_upper_arm_roll_joint\"/>"
00159 "<joint name=\"l_elbow_flex_joint\"/>"
00160 "<joint name=\"l_forearm_roll_joint\"/>"
00161 "<joint name=\"l_wrist_flex_joint\"/>"
00162 "<joint name=\"l_wrist_roll_joint\"/>"
00163 "</group>"
00164 "</robot>";
00165 srdfModel->initString(*urdf_model_, SMODEL2);
00166
00167 robot_model::RobotModelPtr kin_model2(new robot_model::RobotModel(urdf_model_, srdfModel));
00168
00169 left_arm_base_tip_group = kin_model2->getJointModelGroup("left_arm_base_tip");
00170 ASSERT_TRUE(left_arm_base_tip_group != NULL);
00171
00172 left_arm_joints_group = kin_model2->getJointModelGroup("left_arm_joints");
00173 ASSERT_TRUE(left_arm_joints_group != NULL);
00174
00175 EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 7);
00176 EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7);
00177
00178 EXPECT_EQ(left_arm_joints_group->getVariableNames().size(), left_arm_joints_group->getVariableCount());
00179 EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7);
00180
00181 EXPECT_EQ(kin_model2->getVariableNames().size(), kin_model2->getVariableCount());
00182
00183 bool found_shoulder_pan_link = false;
00184 bool found_wrist_roll_link = false;
00185 for(unsigned int i = 0; i < left_arm_base_tip_group->getLinkModels().size(); i++)
00186 {
00187 if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_shoulder_pan_link")
00188 {
00189 EXPECT_TRUE(found_shoulder_pan_link == false);
00190 found_shoulder_pan_link = true;
00191 }
00192 if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_wrist_roll_link")
00193 {
00194 EXPECT_TRUE(found_wrist_roll_link == false);
00195 found_wrist_roll_link = true;
00196 }
00197 EXPECT_TRUE(left_arm_base_tip_group->getLinkModels()[i]->getName() != "torso_lift_link");
00198 }
00199 EXPECT_TRUE(found_shoulder_pan_link);
00200 EXPECT_TRUE(found_wrist_roll_link);
00201
00202
00203 robot_state::RobotState ks(kin_model2);
00204 ks.setToDefaultValues();
00205 std::map<std::string, double> jv;
00206 jv["base_joint.x"] = 0.433;
00207 jv["base_joint.theta"] = -0.5;
00208 ks.setStateValues(jv);
00209 moveit_msgs::RobotState robot_state;
00210 robot_state::robotStateToRobotStateMsg(ks, robot_state);
00211
00212 robot_state::RobotState ks2(kin_model2);
00213 robotStateMsgToRobotState(robot_state, ks2);
00214 std::vector<double> v1;
00215 ks.getStateValues(v1);
00216 std::vector<double> v2;
00217 ks2.getStateValues(v2);
00218 EXPECT_TRUE(v1.size() == v2.size());
00219 for (unsigned int i = 0; i < v1.size(); ++i)
00220 EXPECT_NEAR(v1[i], v2[i], 1e-5);
00221
00222 std::vector<double> state_double_vector;
00223 ks.getStateValues(state_double_vector);
00224 ASSERT_TRUE(ks.setStateValues(state_double_vector));
00225 }
00226
00227 TEST_F(LoadPlanningModelsPr2, SubgroupInit)
00228 {
00229 robot_model::RobotModel kmodel(urdf_model_, srdf_model_);
00230 const robot_model::JointModelGroup* jmg = kmodel.getJointModelGroup("arms");
00231 ASSERT_TRUE(jmg);
00232 EXPECT_EQ(jmg->getSubgroupNames().size(), 2);
00233 EXPECT_TRUE(jmg->isSubgroup("right_arm"));
00234
00235 const robot_model::JointModelGroup* jmg2 = kmodel.getJointModelGroup("whole_body");
00236 EXPECT_EQ(jmg2->getSubgroupNames().size(), 4);
00237 EXPECT_TRUE(jmg2->isSubgroup("arms"));
00238 EXPECT_TRUE(jmg2->isSubgroup("right_arm"));
00239 }
00240
00241 TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks)
00242 {
00243 boost::shared_ptr<robot_model::RobotModel> kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
00244
00245 EXPECT_TRUE(kmodel->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1);
00246 }
00247
00248
00249 TEST_F(LoadPlanningModelsPr2, FullTest)
00250 {
00251 robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
00252
00253 robot_state::RobotState ks(kmodel);
00254 ks.setToDefaultValues();
00255
00256 robot_state::RobotState ks2(kmodel);
00257 ks2.setToDefaultValues();
00258
00259 std::vector<shapes::ShapeConstPtr> shapes;
00260 EigenSTL::vector_Affine3d poses;
00261 shapes::Shape* shape = new shapes::Box(.1,.1,.1);
00262 shapes.push_back(shapes::ShapeConstPtr(shape));
00263 poses.push_back(Eigen::Affine3d::Identity());
00264 std::set<std::string> touch_links;
00265
00266 sensor_msgs::JointState empty_state;
00267 robot_state::AttachedBody attached_body(ks.getLinkState("r_gripper_palm_link")->getLinkModel(), "box", shapes, poses, touch_links, empty_state);
00268
00269 ks.attachBody(&attached_body);
00270
00271 std::vector<const robot_state::AttachedBody*> attached_bodies_1;
00272 ks.getAttachedBodies(attached_bodies_1);
00273 ASSERT_EQ(attached_bodies_1.size(),1);
00274
00275 std::vector<const robot_state::AttachedBody*> attached_bodies_2;
00276 ks2 = ks;
00277 ks2.getAttachedBodies(attached_bodies_2);
00278 ASSERT_EQ(attached_bodies_2.size(),1);
00279
00280 ks.clearAttachedBody("box");
00281 attached_bodies_1.clear();
00282 ks.getAttachedBodies(attached_bodies_1);
00283 ASSERT_EQ(attached_bodies_1.size(),0);
00284
00285 ks2 = ks;
00286 attached_bodies_2.clear();
00287 ks2.getAttachedBodies(attached_bodies_2);
00288 ASSERT_EQ(attached_bodies_2.size(),0);
00289 }
00290
00291 int main(int argc, char **argv)
00292 {
00293 testing::InitGoogleTest(&argc, argv);
00294 return RUN_ALL_TESTS();
00295 }