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