moveit_core: test_kinematic.cpp Source File

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