planning_models: test_kinematic.cpp Source 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 
00043 static bool sameStringIgnoringWS(const std::string &s1, const std::string &s2)
00044 {
00045     unsigned int i1 = 0;
00046     unsigned int i2 = 0;
00047     while (i1 < s1.size() && isspace(s1[i1])) i1++;
00048     while (i2 < s2.size() && isspace(s2[i2])) i2++;
00049     while (i1 < s1.size() && i2 < s2.size())
00050     {
00051         if (i1 < s1.size() && i2 < s2.size())
00052         {
00053             if (s1[i1] != s2[i2])
00054                 return false;
00055             i1++;
00056             i2++;
00057         }
00058         while (i1 < s1.size() && isspace(s1[i1])) i1++;
00059         while (i2 < s2.size() && isspace(s2[i2])) i2++;
00060     }
00061     return i1 == s1.size() && i2 == s2.size();
00062 }
00063 
00064 TEST(Loading, SimpleRobot)
00065 {
00066     static const std::string MODEL0 = 
00067       "<?xml version=\"1.0\" ?>" 
00068       "<robot name=\"myrobot\">" 
00069       "  <link name=\"base_link\">"
00070       "    <collision name=\"base_collision\">"
00071       "    <origin rpy=\"0 0 0\" xyz=\"0 0 0.165\"/>"
00072       "    <geometry name=\"base_collision_geom\">"
00073       "      <box size=\"0.65 0.65 0.23\"/>"
00074       "    </geometry>"
00075       "    </collision>"
00076       "   </link>"
00077       "</robot>";
00078 
00079     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00080     planning_models::KinematicModel::MultiDofConfig config("base_joint");
00081     config.type = "Planar";
00082     config.parent_frame_id = "odom_combined";
00083     config.child_frame_id = "base_link";
00084     multi_dof_configs.push_back(config);
00085 
00086     urdf::Model urdfModel;
00087     urdfModel.initString(MODEL0);
00088     
00089     std::map < std::string, std::vector<std::string> > groups;
00090     boost::shared_ptr<planning_models::KinematicModel> model(new planning_models::KinematicModel(urdfModel,groups,multi_dof_configs));
00091     
00092     planning_models::KinematicState state(model);
00093 
00094     EXPECT_EQ(std::string("myrobot"), model->getName());
00095     EXPECT_EQ((unsigned int)3, state.getDimension());
00096 
00097     const std::vector<planning_models::KinematicModel::LinkModel*>& links = model->getLinkModels();
00098     EXPECT_EQ((unsigned int)1, links.size());
00099     
00100     const std::vector<planning_models::KinematicModel::JointModel*>& joints = model->getJointModels();
00101     EXPECT_EQ((unsigned int)1, joints.size());
00102     
00103     std::vector<std::string> pgroups;
00104     model->getModelGroupNames(pgroups);    
00105     EXPECT_EQ((unsigned int)0, pgroups.size());    
00106 
00107 }
00108 
00109 TEST(LoadingAndFK, SimpleRobot)
00110 {   
00111     static const std::string MODEL1 = 
00112         "<?xml version=\"1.0\" ?>" 
00113       "<robot name=\"myrobot\">" 
00114         "<link name=\"base_link\">"
00115         "  <inertial>"
00116         "    <mass value=\"2.81\"/>"
00117         "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
00118         "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00119         "  </inertial>"
00120         "  <collision name=\"my_collision\">"
00121         "    <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
00122         "    <geometry>"
00123         "      <box size=\"1 2 1\" />"
00124         "    </geometry>"
00125         "  </collision>"
00126         "  <visual>"
00127         "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00128         "    <geometry>"
00129         "      <box size=\"1 2 1\" />"
00130         "    </geometry>"
00131         "  </visual>"
00132         "</link>"
00133         "</robot>";
00134     
00135     static const std::string MODEL1_INFO = 
00136       "Complete model state dimension = 3\n"
00137       "State bounds: [-3.14159, 3.14159] [-DBL_MAX, DBL_MAX] [-DBL_MAX, DBL_MAX]\n"
00138       "Root joint : base_joint \n"
00139       "Available groups: base \n"
00140       "Group base has 1 roots: base_joint \n";
00141 
00142     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00143     planning_models::KinematicModel::MultiDofConfig config("base_joint");
00144     config.type = "Planar";
00145     config.parent_frame_id = "odom_combined";
00146     config.child_frame_id = "base_link";
00147     multi_dof_configs.push_back(config);
00148     
00149     urdf::Model urdfModel;
00150     urdfModel.initString(MODEL1);
00151     
00152     std::map < std::string, std::vector<std::string> > groups;
00153     groups["base"].push_back("base_joint");
00154 
00155     boost::shared_ptr<planning_models::KinematicModel> model(new planning_models::KinematicModel(urdfModel,groups,multi_dof_configs));
00156     
00157     planning_models::KinematicState state(model);
00158 
00159     EXPECT_EQ((unsigned int)3, state.getDimension());
00160     
00161     state.setKinematicStateToDefault();
00162     
00163     const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector();
00164     EXPECT_EQ((unsigned int)1, joint_states.size());
00165     EXPECT_EQ((unsigned int)3, joint_states[0]->getJointStateValues().size());
00166 
00167     std::stringstream ssi;
00168     state.printStateInfo(ssi);
00169     EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str();
00170         
00171     
00172     std::map<std::string, double> joint_values;
00173     joint_values["planar_x"]=10.0;
00174     joint_values["planar_y"]=8.0;
00175     joint_values["planar_th"]=0.0;
00176     state.setKinematicState(joint_values);
00177 
00178     EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00179     EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00180     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00181 
00182     const std::map<std::string, unsigned int>& ind_map = state.getKinematicStateIndexMap();
00183     std::vector<double> jv(state.getDimension(), 0.0);
00184     jv[ind_map.at("planar_x")] = 10.0;
00185     jv[ind_map.at("planar_y")] = 8.0;
00186     jv[ind_map.at("planar_th")] = 0.0;
00187 
00188     state.setKinematicState(jv);
00189     EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00190     EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00191     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00192 }
00193 
00194 TEST(FK, OneRobot)
00195 {
00196     static const std::string MODEL2 = 
00197       "<?xml version=\"1.0\" ?>" 
00198       "<robot name=\"one_robot\">"
00199       "<link name=\"base_link\">"
00200       "  <inertial>"
00201       "    <mass value=\"2.81\"/>"
00202       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00203       "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00204       "  </inertial>"
00205       "  <collision name=\"my_collision\">"
00206       "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00207       "    <geometry>"
00208       "      <box size=\"1 2 1\" />"
00209       "    </geometry>"
00210       "  </collision>"
00211       "  <visual>"
00212       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00213       "    <geometry>"
00214       "      <box size=\"1 2 1\" />"
00215       "    </geometry>"
00216       "  </visual>"
00217       "</link>"
00218       "<joint name=\"joint_a\" type=\"continuous\">"
00219       "   <axis xyz=\"0 0 1\"/>"
00220       "   <parent link=\"base_link\"/>"
00221       "   <child link=\"link_a\"/>"
00222       "   <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
00223       "</joint>"
00224       "<link name=\"link_a\">"
00225       "  <inertial>"
00226       "    <mass value=\"1.0\"/>"
00227       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00228       "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00229       "  </inertial>"
00230       "  <collision>"
00231       "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00232       "    <geometry>"
00233       "      <box size=\"1 2 1\" />"
00234       "    </geometry>"
00235       "  </collision>"
00236       "  <visual>"
00237       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00238       "    <geometry>"
00239       "      <box size=\"1 2 1\" />"
00240       "    </geometry>"
00241       "  </visual>"
00242       "</link>"
00243       "<joint name=\"joint_b\" type=\"fixed\">"
00244       "  <parent link=\"link_a\"/>"
00245       "  <child link=\"link_b\"/>"
00246       "  <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
00247       "</joint>"
00248       "<link name=\"link_b\">"
00249       "  <inertial>"
00250       "    <mass value=\"1.0\"/>"
00251       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00252       "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00253       "  </inertial>"
00254       "  <collision>"
00255       "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00256       "    <geometry>"
00257       "      <box size=\"1 2 1\" />"
00258       "    </geometry>"
00259       "  </collision>"
00260       "  <visual>"
00261       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00262       "    <geometry>"
00263       "      <box size=\"1 2 1\" />"
00264       "    </geometry>"
00265       "  </visual>"
00266       "</link>"
00267       "  <joint name=\"joint_c\" type=\"prismatic\">"
00268       "    <axis xyz=\"1 0 0\"/>"
00269       "    <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
00270       "    <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" soft_upper_limit=\"0.089\"/>"
00271       "    <parent link=\"link_b\"/>"
00272       "    <child link=\"link_c\"/>"
00273       "    <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
00274       "  </joint>"
00275       "<link name=\"link_c\">"
00276       "  <inertial>"
00277       "    <mass value=\"1.0\"/>"
00278       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
00279       "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00280       "  </inertial>"
00281       "  <collision>"
00282       "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00283       "    <geometry>"
00284       "      <box size=\"1 2 1\" />"
00285       "    </geometry>"
00286       "  </collision>"
00287       "  <visual>"
00288       "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00289       "    <geometry>"
00290       "      <box size=\"1 2 1\" />"
00291       "    </geometry>"
00292       "  </visual>"
00293       "</link>"
00294       "</robot>";
00295     
00296     static const std::string MODEL2_INFO = 
00297       "Complete model state dimension = 5\n"
00298       "State bounds: [-3.14159, 3.14159] [-DBL_MAX, DBL_MAX] [-DBL_MAX, DBL_MAX] [-3.14159, 3.14159] [0.00000, 0.08900]\n"
00299       "Root joint : base_joint \n"
00300       "Available groups: base \n"
00301       "Group base has 1 roots: base_joint \n";
00302 
00303     urdf::Model urdfModel;
00304     urdfModel.initString(MODEL2);
00305     
00306     std::map < std::string, std::vector<std::string> > groups;
00307     groups["base"].push_back("base_joint");
00308     groups["base"].push_back("joint_a");
00309     groups["base"].push_back("joint_b");
00310     groups["base"].push_back("joint_c");
00311 
00312     std::vector<planning_models::KinematicModel::MultiDofConfig> multi_dof_configs;
00313     planning_models::KinematicModel::MultiDofConfig config("base_joint");
00314     config.type = "Planar";
00315     config.parent_frame_id = "odom_combined";
00316     config.child_frame_id = "base_link";
00317     multi_dof_configs.push_back(config);
00318     
00319     boost::shared_ptr<planning_models::KinematicModel> model(new planning_models::KinematicModel(urdfModel,groups,multi_dof_configs));
00320     
00321     planning_models::KinematicState state(model);
00322 
00323     EXPECT_EQ((unsigned int)5, state.getDimension());
00324 
00325     std::map<std::string, double> joint_values;
00326     joint_values["planar_x"]=1.0;
00327     joint_values["planar_y"]=1.0;
00328     joint_values["planar_th"]=0.5;
00329     joint_values["joint_a"] = -0.5;
00330     joint_values["joint_c"] = 0.1;
00331     state.getJointStateGroup("base")->setKinematicState(joint_values);
00332 
00333     //double param[5] = { 1, 1, 0.5, -0.5, 0.1 };
00334     //model->getGroup("base")->computeTransforms(param);
00335     
00336     std::stringstream ss1;
00337     state.printStateInfo(ss1);
00338     EXPECT_TRUE(sameStringIgnoringWS(MODEL2_INFO, ss1.str())) << ss1.str();
00339     state.printTransforms(ss1);
00340 
00341     //make sure it works the same way for the whole robot
00342     state.setKinematicState(joint_values);
00343     std::stringstream ss2;
00344     state.printStateInfo(ss2);
00345     state.printTransforms(ss2);
00346     
00347     EXPECT_EQ(ss1.str(), ss2.str());
00348     
00349     EXPECT_NEAR(1.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00350     EXPECT_NEAR(1.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00351     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00352     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00353     EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00354     EXPECT_NEAR(0.247404, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00355     EXPECT_NEAR(0.968912, state.getLinkState("base_link")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00356 
00357     EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00358     EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00359     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00360     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00361     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00362     EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00363     EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00364 
00365     EXPECT_NEAR(1.0, state.getLinkState("link_b")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00366     EXPECT_NEAR(1.5, state.getLinkState("link_b")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00367     EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00368     EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00369     EXPECT_NEAR(-0.20846, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00370     EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00371     EXPECT_NEAR(0.978031, state.getLinkState("link_b")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00372 
00373     EXPECT_NEAR(1.1, state.getLinkState("link_c")->getGlobalLinkTransform().getOrigin().x(), 1e-5);
00374     EXPECT_NEAR(1.4, state.getLinkState("link_c")->getGlobalLinkTransform().getOrigin().y(), 1e-5);
00375     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getOrigin().z(), 1e-5);
00376     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().x(), 1e-5);
00377     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().y(), 1e-5);
00378     EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().z(), 1e-5);
00379     EXPECT_NEAR(1.0, state.getLinkState("link_c")->getGlobalLinkTransform().getRotation().w(), 1e-5);
00380 
00381     //bonus bounds lookup test
00382     std::vector<std::string> jn;
00383     jn.push_back("planar_x");
00384     jn.push_back("planar_th");
00385     jn.push_back("base_joint");
00386     EXPECT_TRUE(state.areJointsWithinBounds(jn));
00387 
00388     jn.push_back("monkey");
00389     EXPECT_FALSE(state.areJointsWithinBounds(jn));
00390 }
00391 int main(int argc, char **argv)
00392 {
00393     testing::InitGoogleTest(&argc, argv);
00394     return RUN_ALL_TESTS();
00395 }