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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53