test_kinematic.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, 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 
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 <geometric_shapes/shape_operations.h>
00043 
00044 static bool sameStringIgnoringWS(const std::string &s1, const std::string &s2)
00045 {
00046   unsigned int i1 = 0;
00047   unsigned int i2 = 0;
00048   while (i1 < s1.size() && isspace(s1[i1])) i1++;
00049   while (i2 < s2.size() && isspace(s2[i2])) i2++;
00050   while (i1 < s1.size() && i2 < s2.size())
00051   {
00052     if (i1 < s1.size() && i2 < s2.size())
00053     {
00054       if (s1[i1] != s2[i2])
00055         return false;
00056       i1++;
00057       i2++;
00058     }
00059     while (i1 < s1.size() && isspace(s1[i1])) i1++;
00060     while (i2 < s2.size() && isspace(s2[i2])) i2++;
00061   }
00062   return i1 == s1.size() && i2 == s2.size();
00063 }
00064 
00065 TEST(Loading, SimpleRobot)
00066 {
00067   static const std::string MODEL0 = 
00068     "<?xml version=\"1.0\" ?>" 
00069     "<robot name=\"myrobot\">" 
00070     "  <link name=\"base_link\">"
00071     "    <collision name=\"base_collision\">"
00072     "    <origin rpy=\"0 0 0\" xyz=\"0 0 0.165\"/>"
00073     "    <geometry name=\"base_collision_geom\">"
00074     "      <box size=\"0.65 0.65 0.23\"/>"
00075     "    </geometry>"
00076     "    </collision>"
00077     "   </link>"
00078     "</robot>";
00079 
00080   std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00081   planning_models::KinematicModel::MultiDofConfig config("base_joint");
00082   config.type = "Floating";
00083   config.parent_frame_id = "odom_combined";
00084   config.child_frame_id = "base_link";
00085   multi_dof_configs.push_back(config);
00086 
00087   urdf::Model urdfModel;
00088   urdfModel.initString(MODEL0);
00089     
00090   std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00091   planning_models::KinematicModel* model = new planning_models::KinematicModel(urdfModel,gcs,multi_dof_configs);
00092  
00093   //bracketing so the state gets destroyed before we bring down the model
00094   {
00095     planning_models::KinematicState state(model);
00096 
00097     state.setKinematicStateToDefault();
00098 
00099     //make sure that this copy constructor works
00100     planning_models::KinematicState new_state(state);
00101 
00102     //(0,0,0,0) isn't a valid quaternion, so the w should be 1
00103     std::map<std::string, double> state_values;
00104     new_state.getKinematicStateValues(state_values);
00105 
00106     EXPECT_EQ(state_values["floating_rot_w"], 1.0);
00107 
00108     EXPECT_EQ(std::string("myrobot"), model->getName());
00109     EXPECT_EQ((unsigned int)7, new_state.getDimension());
00110     
00111     const std::vector<planning_models::KinematicModel::LinkModel*>& links = model->getLinkModels();
00112     EXPECT_EQ((unsigned int)1, links.size());
00113     
00114     const std::vector<planning_models::KinematicModel::JointModel*>& joints = model->getJointModels();
00115     EXPECT_EQ((unsigned int)1, joints.size());
00116     
00117     std::vector<std::string> pgroups;
00118     model->getModelGroupNames(pgroups);    
00119     EXPECT_EQ((unsigned int)0, pgroups.size());
00120   }
00121 
00122   delete model;
00123 }
00124 
00125 TEST(LoadingAndFK, SimpleRobot)
00126 {   
00127   static const std::string MODEL1 = 
00128     "<?xml version=\"1.0\" ?>" 
00129     "<robot name=\"myrobot\">" 
00130     "<link name=\"base_link\">"
00131     "  <inertial>"
00132     "    <mass value=\"2.81\"/>"
00133     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
00134     "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00135     "  </inertial>"
00136     "  <collision name=\"my_collision\">"
00137     "    <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
00138     "    <geometry>"
00139     "      <box size=\"1 2 1\" />"
00140     "    </geometry>"
00141     "  </collision>"
00142     "  <visual>"
00143     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00144     "    <geometry>"
00145     "      <box size=\"1 2 1\" />"
00146     "    </geometry>"
00147     "  </visual>"
00148     "</link>"
00149     "</robot>";
00150     
00151   static const std::string MODEL1_INFO = 
00152     "Complete model state dimension = 3\n"
00153     "State bounds: [-3.14159, 3.14159] [-DBL_MAX, DBL_MAX] [-DBL_MAX, DBL_MAX]\n"
00154     "Root joint : base_joint \n"
00155     "Available groups: base \n"
00156     "Group base has 1 roots: base_joint \n";
00157 
00158   std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00159   planning_models::KinematicModel::MultiDofConfig config("base_joint");
00160   config.type = "Planar";
00161   config.parent_frame_id = "odom_combined";
00162   config.child_frame_id = "base_link";
00163   multi_dof_configs.push_back(config);
00164     
00165   urdf::Model urdfModel;
00166   urdfModel.initString(MODEL1);
00167 
00168   std::vector<std::string> gc_joints;
00169   gc_joints.push_back("base_joint");
00170   std::vector<std::string> empty;
00171   std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00172   gcs.push_back(planning_models::KinematicModel::GroupConfig("base", gc_joints, empty));
00173 
00174   planning_models::KinematicModel* model(new planning_models::KinematicModel(urdfModel,gcs,multi_dof_configs));
00175 
00176   //bracketing so the state gets destroyed before we bring down the model
00177   {    
00178     planning_models::KinematicState state(model);
00179     
00180     EXPECT_EQ((unsigned int)3, state.getDimension());
00181     
00182     state.setKinematicStateToDefault();
00183 
00184     const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector();
00185     EXPECT_EQ((unsigned int)1, joint_states.size());
00186     EXPECT_EQ((unsigned int)3, joint_states[0]->getJointStateValues().size());
00187     
00188     std::stringstream ssi;
00189     state.printStateInfo(ssi);
00190     EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str();
00191     
00192     
00193     std::map<std::string, double> joint_values;
00194     joint_values["planar_x"]=10.0;
00195     joint_values["planar_y"]=8.0;
00196     
00197     //testing incomplete state
00198     std::vector<std::string> missing_states;
00199     EXPECT_FALSE(state.setKinematicState(joint_values,
00200                                          missing_states));
00201     ASSERT_EQ(missing_states.size(),1);
00202     EXPECT_EQ(missing_states[0],std::string("planar_th"));
00203     joint_values["planar_th"]=0.0;
00204 
00205     EXPECT_TRUE(state.setKinematicState(joint_values,
00206                                          missing_states));
00207     ASSERT_EQ(missing_states.size(),0);
00208 
00209     EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00210     EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00211     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00212 
00213     //making sure that values get copied
00214     planning_models::KinematicState new_state(state);
00215     EXPECT_NEAR(10.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00216     EXPECT_NEAR(8.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00217     EXPECT_NEAR(0.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00218     
00219     const std::map<std::string, unsigned int>& ind_map = state.getKinematicStateIndexMap();
00220     std::vector<double> jv(state.getDimension(), 0.0);
00221     jv[ind_map.at("planar_x")] = 10.0;
00222     jv[ind_map.at("planar_y")] = 8.0;
00223     jv[ind_map.at("planar_th")] = 0.0;
00224     
00225     state.setKinematicState(jv);
00226     EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00227     EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00228     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00229   } 
00230   delete model;
00231 }
00232 
00233 TEST(FK, OneRobot)
00234 {
00235   static const std::string MODEL2 = 
00236     "<?xml version=\"1.0\" ?>" 
00237     "<robot name=\"one_robot\">"
00238     "<link name=\"base_link\">"
00239     "  <inertial>"
00240     "    <mass value=\"2.81\"/>"
00241     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00242     "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00243     "  </inertial>"
00244     "  <collision name=\"my_collision\">"
00245     "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00246     "    <geometry>"
00247     "      <box size=\"1 2 1\" />"
00248     "    </geometry>"
00249     "  </collision>"
00250     "  <visual>"
00251     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00252     "    <geometry>"
00253     "      <box size=\"1 2 1\" />"
00254     "    </geometry>"
00255     "  </visual>"
00256     "</link>"
00257     "<joint name=\"joint_a\" type=\"continuous\">"
00258     "   <axis xyz=\"0 0 1\"/>"
00259     "   <parent link=\"base_link\"/>"
00260     "   <child link=\"link_a\"/>"
00261     "   <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
00262     "</joint>"
00263     "<link name=\"link_a\">"
00264     "  <inertial>"
00265     "    <mass value=\"1.0\"/>"
00266     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00267     "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00268     "  </inertial>"
00269     "  <collision>"
00270     "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00271     "    <geometry>"
00272     "      <box size=\"1 2 1\" />"
00273     "    </geometry>"
00274     "  </collision>"
00275     "  <visual>"
00276     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00277     "    <geometry>"
00278     "      <box size=\"1 2 1\" />"
00279     "    </geometry>"
00280     "  </visual>"
00281     "</link>"
00282     "<joint name=\"joint_b\" type=\"fixed\">"
00283     "  <parent link=\"link_a\"/>"
00284     "  <child link=\"link_b\"/>"
00285     "  <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
00286     "</joint>"
00287     "<link name=\"link_b\">"
00288     "  <inertial>"
00289     "    <mass value=\"1.0\"/>"
00290     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00291     "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00292     "  </inertial>"
00293     "  <collision>"
00294     "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00295     "    <geometry>"
00296     "      <box size=\"1 2 1\" />"
00297     "    </geometry>"
00298     "  </collision>"
00299     "  <visual>"
00300     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00301     "    <geometry>"
00302     "      <box size=\"1 2 1\" />"
00303     "    </geometry>"
00304     "  </visual>"
00305     "</link>"
00306     "  <joint name=\"joint_c\" type=\"prismatic\">"
00307     "    <axis xyz=\"1 0 0\"/>"
00308     "    <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
00309     "    <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" soft_upper_limit=\"0.089\"/>"
00310     "    <parent link=\"link_b\"/>"
00311     "    <child link=\"link_c\"/>"
00312     "    <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
00313     "  </joint>"
00314     "<link name=\"link_c\">"
00315     "  <inertial>"
00316     "    <mass value=\"1.0\"/>"
00317     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
00318     "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00319     "  </inertial>"
00320     "  <collision>"
00321     "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00322     "    <geometry>"
00323     "      <box size=\"1 2 1\" />"
00324     "    </geometry>"
00325     "  </collision>"
00326     "  <visual>"
00327     "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00328     "    <geometry>"
00329     "      <box size=\"1 2 1\" />"
00330     "    </geometry>"
00331     "  </visual>"
00332     "</link>"
00333     "</robot>";
00334     
00335   static const std::string MODEL2_INFO = 
00336     "Complete model state dimension = 5\n"
00337     "State bounds: [-3.14159, 3.14159] [-DBL_MAX, DBL_MAX] [-DBL_MAX, DBL_MAX] [-3.14159, 3.14159] [0.00000, 0.08900]\n"
00338     "Root joint : base_joint \n"
00339     "Available groups: base_from_base_to_tip base_from_joints base_with_subgroups \n"
00340     "Group base_from_base_to_tip has 1 roots: base_joint  \n"
00341     "Group base_from_joints has 1 roots: base_joint \n"
00342     "Group base_with_subgroups has 1 roots: base_joint \n";
00343     
00344 
00345   urdf::Model urdfModel;
00346   urdfModel.initString(MODEL2);
00347 
00348   std::vector<std::string> gc_joints;
00349   gc_joints.push_back("base_joint");
00350   gc_joints.push_back("joint_a");
00351   gc_joints.push_back("joint_c");
00352   std::vector<std::string> subgroups;
00353   std::vector<planning_models::KinematicModel::GroupConfig> gcs;
00354   gcs.push_back(planning_models::KinematicModel::GroupConfig("base_from_joints", gc_joints, subgroups));
00355 
00356   //defining this in the list before the actual subgroup to make sure that works
00357   std::vector<std::string> extra_joint;
00358   extra_joint.push_back("joint_c");
00359   subgroups.push_back("base_from_base_to_tip");
00360   gcs.push_back(planning_models::KinematicModel::GroupConfig("base_with_subgroups", extra_joint, subgroups));
00361     
00362   //not making this all the way to joint_c intentionally
00363   gcs.push_back(planning_models::KinematicModel::GroupConfig("base_from_base_to_tip","odom_combined","link_b"));
00364 
00365   //now adding a fake one
00366   subgroups.push_back("monkey_group");
00367   gcs.push_back(planning_models::KinematicModel::GroupConfig("base_with_bad_subgroups", extra_joint, subgroups));
00368 
00369   std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00370   planning_models::KinematicModel::MultiDofConfig config("base_joint");
00371   config.type = "Planar";
00372   config.parent_frame_id = "odom_combined";
00373   config.child_frame_id = "base_link";
00374   multi_dof_configs.push_back(config);
00375     
00376   planning_models::KinematicModel* model = new planning_models::KinematicModel(urdfModel,gcs,multi_dof_configs);
00377     
00378   //testing that the two planning groups are the same
00379   const planning_models::KinematicModel::JointModelGroup* g_one = model->getModelGroup("base_from_joints");
00380   const planning_models::KinematicModel::JointModelGroup* g_two = model->getModelGroup("base_from_base_to_tip");
00381   const planning_models::KinematicModel::JointModelGroup* g_three = model->getModelGroup("base_with_subgroups");
00382   const planning_models::KinematicModel::JointModelGroup* g_four = model->getModelGroup("base_with_bad_subgroups");
00383 
00384   ASSERT_TRUE(g_one != NULL);
00385   ASSERT_TRUE(g_two != NULL);
00386   ASSERT_TRUE(g_three != NULL);
00387   ASSERT_TRUE(g_four == NULL);
00388 
00389   //joint_b is a fixed joint, so no one should have it
00390   ASSERT_EQ(g_one->getJointModelNames().size(), 3); 
00391   ASSERT_EQ(g_two->getJointModelNames().size(), 2); 
00392   ASSERT_EQ(g_three->getJointModelNames().size(), 3); 
00393 
00394   //only the links in between the joints, and the children of the leafs
00395   ASSERT_EQ(g_one->getGroupLinkModels().size(), 4);
00396   //g_two only has three links
00397   ASSERT_EQ(g_two->getGroupLinkModels().size(), 3);
00398   ASSERT_EQ(g_three->getGroupLinkModels().size(), 4);
00399     
00400   EXPECT_EQ(g_one->getJointModelNames()[0],"base_joint");
00401   EXPECT_EQ(g_one->getJointModelNames()[1],"joint_a");
00402   EXPECT_EQ(g_one->getJointModelNames()[2],"joint_c");
00403   EXPECT_EQ(g_two->getJointModelNames()[0],"base_joint");
00404   EXPECT_EQ(g_two->getJointModelNames()[1],"joint_a");
00405   EXPECT_EQ(g_three->getJointModelNames()[0],"base_joint");
00406   EXPECT_EQ(g_three->getJointModelNames()[1],"joint_a");
00407   EXPECT_EQ(g_three->getJointModelNames()[2],"joint_c");
00408 
00409   //but they should have the same links to be updated
00410   ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 4);
00411   ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 4);
00412   ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 4);
00413 
00414   EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(),"base_link");
00415   EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(),"link_a");
00416   EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(),"link_b");
00417   EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(),"link_c");
00418 
00419   EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(),"base_link");
00420   EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(),"link_a");
00421   EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(),"link_b");
00422   EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(),"link_c");
00423 
00424   EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(),"base_link");
00425   EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(),"link_a");
00426   EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(),"link_b");
00427   EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(),"link_c");
00428 
00429   //bracketing so the state gets destroyed before we bring down the model
00430   {
00431     planning_models::KinematicState state(model);
00432     
00433     EXPECT_EQ((unsigned int)5, state.getDimension());
00434 
00435     state.setKinematicStateToDefault();
00436 
00437     std::map<std::string, double> joint_values;
00438     joint_values["planar_x"]=1.0;
00439     joint_values["planar_y"]=1.0;
00440     joint_values["planar_th"]=0.5;
00441     joint_values["joint_a"] = -0.5;
00442     joint_values["joint_c"] = 0.1;
00443     state.getJointStateGroup("base_from_joints")->setKinematicState(joint_values);
00444     
00445     //double param[5] = { 1, 1, 0.5, -0.5, 0.1 };
00446     //model->getGroup("base")->computeTransforms(param);
00447     
00448     std::stringstream ss1;
00449     state.printStateInfo(ss1);
00450     EXPECT_TRUE(sameStringIgnoringWS(MODEL2_INFO, ss1.str())) << MODEL2_INFO << "\n" << ss1.str();
00451     state.printTransforms(ss1);
00452     
00453     //make sure it works the same way for the whole robot
00454     state.setKinematicState(joint_values);
00455     std::stringstream ss2;
00456     state.printStateInfo(ss2);
00457     state.printTransforms(ss2);
00458     
00459     EXPECT_EQ(ss1.str(), ss2.str());
00460     
00461     EXPECT_NEAR(1.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00462     EXPECT_NEAR(1.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00463     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00464     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00465     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00466     EXPECT_NEAR(0.247404, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00467     EXPECT_NEAR(0.968912, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00468     
00469     EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00470     EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00471     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00472     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00473     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00474     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00475     EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00476     
00477     EXPECT_NEAR(1.0, state.getLinkState("link_b")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00478     EXPECT_NEAR(1.5, state.getLinkState("link_b")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00479     EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00480     EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00481     EXPECT_NEAR(-0.20846, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00482     EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00483     EXPECT_NEAR(0.978031, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00484     
00485     EXPECT_NEAR(1.1, state.getLinkState("link_c")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00486     EXPECT_NEAR(1.4, state.getLinkState("link_c")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00487     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00488     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00489     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00490     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00491     EXPECT_NEAR(1.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00492 
00493     //bonus bounds lookup test
00494     std::vector<std::string> jn;
00495     jn.push_back("planar_x");
00496     jn.push_back("planar_th");
00497     jn.push_back("base_joint");
00498     EXPECT_TRUE(state.areJointsWithinBounds(jn));
00499     
00500     jn.push_back("monkey");
00501     EXPECT_FALSE(state.areJointsWithinBounds(jn));
00502   }
00503 
00504   delete model;
00505 }
00506 int main(int argc, char **argv)
00507 {
00508   testing::InitGoogleTest(&argc, argv);
00509   return RUN_ALL_TESTS();
00510 }


planning_models
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Mon Dec 2 2013 12:33:38