test_kinematic_complex.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, 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 
00035 /* Author: Ioan Sucan */
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   // with no world multidof we should get a fixed joint
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44