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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49