00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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])) 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 moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
00094 moveit::core::RobotState state(model);
00095
00096 state.setToDefaultValues();
00097
00098
00099 moveit::core::RobotState new_state(state);
00100
00101 EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
00102
00103 EXPECT_EQ(std::string("myrobot"), model->getName());
00104 EXPECT_EQ((unsigned int)7, new_state.getVariableCount());
00105
00106 const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
00107 EXPECT_EQ((unsigned int)1, links.size());
00108
00109 const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
00110 EXPECT_EQ((unsigned int)1, joints.size());
00111
00112 const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
00113 EXPECT_EQ((unsigned int)0, pgroups.size());
00114 }
00115
00116 TEST(LoadingAndFK, SimpleRobot)
00117 {
00118 static const std::string MODEL1 =
00119 "<?xml version=\"1.0\" ?>"
00120 "<robot name=\"myrobot\">"
00121 "<link name=\"base_link\">"
00122 " <inertial>"
00123 " <mass value=\"2.81\"/>"
00124 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
00125 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00126 " </inertial>"
00127 " <collision name=\"my_collision\">"
00128 " <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
00129 " <geometry>"
00130 " <box size=\"1 2 1\" />"
00131 " </geometry>"
00132 " </collision>"
00133 " <visual>"
00134 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00135 " <geometry>"
00136 " <box size=\"1 2 1\" />"
00137 " </geometry>"
00138 " </visual>"
00139 "</link>"
00140 "</robot>";
00141
00142 static const std::string SMODEL1 =
00143 "<?xml version=\"1.0\" ?>"
00144 "<robot name=\"myrobot\">"
00145 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
00146 "<group name=\"base\">"
00147 "<joint name=\"base_joint\"/>"
00148 "</group>"
00149 "</robot>";
00150
00151 boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL1);
00152
00153 boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
00154 srdfModel->initString(*urdfModel, SMODEL1);
00155
00156 moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
00157 moveit::core::RobotState state(model);
00158
00159 EXPECT_EQ((unsigned int)3, state.getVariableCount());
00160
00161 state.setToDefaultValues();
00162
00163 EXPECT_EQ((unsigned int)1, (unsigned int)model->getJointModelCount());
00164 EXPECT_EQ((unsigned int)3, (unsigned int)model->getJointModels()[0]->getLocalVariableNames().size());
00165
00166
00167 std::map<std::string, double> joint_values;
00168 joint_values["base_joint/x"] = 10.0;
00169 joint_values["base_joint/y"] = 8.0;
00170
00171
00172 std::vector<std::string> missing_states;
00173 state.setVariablePositions(joint_values, missing_states);
00174 ASSERT_EQ(missing_states.size(), 1);
00175 EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
00176 joint_values["base_joint/theta"] = 0.1;
00177
00178 state.setVariablePositions(joint_values, missing_states);
00179 ASSERT_EQ(missing_states.size(), 0);
00180
00181 EXPECT_NEAR(10.0, state.getGlobalLinkTransform("base_link").translation().x(), 1e-5);
00182 EXPECT_NEAR(8.0, state.getGlobalLinkTransform("base_link").translation().y(), 1e-5);
00183 EXPECT_NEAR(0.0, state.getGlobalLinkTransform("base_link").translation().z(), 1e-5);
00184
00185 state.setVariableAcceleration("base_joint/x", 0.0);
00186
00187
00188 moveit::core::RobotState *new_state = new moveit::core::RobotState(state);
00189 EXPECT_NEAR(10.0, new_state->getGlobalLinkTransform("base_link").translation().x(), 1e-5);
00190 EXPECT_NEAR(8.0, new_state->getGlobalLinkTransform("base_link").translation().y(), 1e-5);
00191 EXPECT_NEAR(0.0, new_state->getGlobalLinkTransform("base_link").translation().z(), 1e-5);
00192 delete new_state;
00193
00194 std::vector<double> jv(state.getVariableCount(), 0.0);
00195 jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 10.0;
00196 jv[state.getRobotModel()->getVariableIndex("base_joint/y")] = 8.0;
00197 jv[state.getRobotModel()->getVariableIndex("base_joint/theta")] = 0.0;
00198
00199 state.setVariablePositions(jv);
00200 EXPECT_NEAR(10.0, state.getGlobalLinkTransform("base_link").translation().x(), 1e-5);
00201 EXPECT_NEAR(8.0, state.getGlobalLinkTransform("base_link").translation().y(), 1e-5);
00202 EXPECT_NEAR(0.0, state.getGlobalLinkTransform("base_link").translation().z(), 1e-5);
00203 }
00204
00205 TEST(FK, OneRobot)
00206 {
00207 static const std::string MODEL2 =
00208 "<?xml version=\"1.0\" ?>"
00209 "<robot name=\"one_robot\">"
00210 "<link name=\"base_link\">"
00211 " <inertial>"
00212 " <mass value=\"2.81\"/>"
00213 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00214 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00215 " </inertial>"
00216 " <collision name=\"my_collision\">"
00217 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00218 " <geometry>"
00219 " <box size=\"1 2 1\" />"
00220 " </geometry>"
00221 " </collision>"
00222 " <visual>"
00223 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00224 " <geometry>"
00225 " <box size=\"1 2 1\" />"
00226 " </geometry>"
00227 " </visual>"
00228 "</link>"
00229 "<joint name=\"joint_a\" type=\"continuous\">"
00230 " <axis xyz=\"0 0 1\"/>"
00231 " <parent link=\"base_link\"/>"
00232 " <child link=\"link_a\"/>"
00233 " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
00234 "</joint>"
00235 "<link name=\"link_a\">"
00236 " <inertial>"
00237 " <mass value=\"1.0\"/>"
00238 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
00239 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00240 " </inertial>"
00241 " <collision>"
00242 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00243 " <geometry>"
00244 " <box size=\"1 2 1\" />"
00245 " </geometry>"
00246 " </collision>"
00247 " <visual>"
00248 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00249 " <geometry>"
00250 " <box size=\"1 2 1\" />"
00251 " </geometry>"
00252 " </visual>"
00253 "</link>"
00254 "<joint name=\"joint_b\" type=\"fixed\">"
00255 " <parent link=\"link_a\"/>"
00256 " <child link=\"link_b\"/>"
00257 " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
00258 "</joint>"
00259 "<link name=\"link_b\">"
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_c\" type=\"prismatic\">"
00279 " <axis xyz=\"1 0 0\"/>"
00280 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
00281 " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" soft_upper_limit=\"0.089\"/>"
00282 " <parent link=\"link_b\"/>"
00283 " <child link=\"link_c\"/>"
00284 " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
00285 " </joint>"
00286 "<link name=\"link_c\">"
00287 " <inertial>"
00288 " <mass value=\"1.0\"/>"
00289 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
00290 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
00291 " </inertial>"
00292 " <collision>"
00293 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00294 " <geometry>"
00295 " <box size=\"1 2 1\" />"
00296 " </geometry>"
00297 " </collision>"
00298 " <visual>"
00299 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
00300 " <geometry>"
00301 " <box size=\"1 2 1\" />"
00302 " </geometry>"
00303 " </visual>"
00304 "</link>"
00305 " <joint name=\"mim_f\" type=\"prismatic\">"
00306 " <axis xyz=\"1 0 0\"/>"
00307 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
00308 " <parent link=\"link_c\"/>"
00309 " <child link=\"link_d\"/>"
00310 " <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
00311 " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
00312 " </joint>"
00313 " <joint name=\"joint_f\" type=\"prismatic\">"
00314 " <axis xyz=\"1 0 0\"/>"
00315 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
00316 " <parent link=\"link_d\"/>"
00317 " <child link=\"link_e\"/>"
00318 " <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
00319 " </joint>"
00320 "<link name=\"link_d\">"
00321 " <collision>"
00322 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00323 " <geometry>"
00324 " <box size=\"1 2 1\" />"
00325 " </geometry>"
00326 " </collision>"
00327 " <visual>"
00328 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
00329 " <geometry>"
00330 " <box size=\"1 2 1\" />"
00331 " </geometry>"
00332 " </visual>"
00333 "</link>"
00334 "<link name=\"link_e\">"
00335 " <collision>"
00336 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
00337 " <geometry>"
00338 " <box size=\"1 2 1\" />"
00339 " </geometry>"
00340 " </collision>"
00341 " <visual>"
00342 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
00343 " <geometry>"
00344 " <box size=\"1 2 1\" />"
00345 " </geometry>"
00346 " </visual>"
00347 "</link>"
00348 "</robot>";
00349
00350 static const std::string SMODEL2 =
00351 "<?xml version=\"1.0\" ?>"
00352 "<robot name=\"one_robot\">"
00353 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
00354 "<group name=\"base_from_joints\">"
00355 "<joint name=\"base_joint\"/>"
00356 "<joint name=\"joint_a\"/>"
00357 "<joint name=\"joint_c\"/>"
00358 "</group>"
00359 "<group name=\"mim_joints\">"
00360 "<joint name=\"joint_f\"/>"
00361 "<joint name=\"mim_f\"/>"
00362 "</group>"
00363 "<group name=\"base_with_subgroups\">"
00364 "<group name=\"base_from_base_to_tip\"/>"
00365 "<joint name=\"joint_c\"/>"
00366 "</group>"
00367 "<group name=\"base_from_base_to_tip\">"
00368 "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
00369 "<joint name=\"base_joint\"/>"
00370 "</group>"
00371 "<group name=\"base_with_bad_subgroups\">"
00372 "<group name=\"error\"/>"
00373 "</group>"
00374 "</robot>";
00375
00376 boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL2);
00377
00378 boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model());
00379 srdfModel->initString(*urdfModel, SMODEL2);
00380
00381 moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdfModel, srdfModel));
00382
00383
00384 const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
00385 const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
00386 const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
00387 const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
00388 const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
00389
00390 ASSERT_TRUE(g_one != NULL);
00391 ASSERT_TRUE(g_two != NULL);
00392 ASSERT_TRUE(g_three != NULL);
00393 ASSERT_TRUE(g_four == NULL);
00394
00395
00396 ASSERT_EQ(g_one->getJointModelNames().size(), 3);
00397 ASSERT_EQ(g_two->getJointModelNames().size(), 3);
00398 ASSERT_EQ(g_three->getJointModelNames().size(), 4);
00399 ASSERT_EQ(g_mim->getJointModelNames().size(), 2);
00400
00401
00402 ASSERT_EQ(g_one->getLinkModelNames().size(), 3);
00403
00404 ASSERT_EQ(g_two->getLinkModelNames().size(), 3);
00405 ASSERT_EQ(g_three->getLinkModelNames().size(), 4);
00406
00407 std::vector<std::string> jmn = g_one->getJointModelNames();
00408 std::sort(jmn.begin(), jmn.end());
00409 EXPECT_EQ(jmn[0],"base_joint");
00410 EXPECT_EQ(jmn[1],"joint_a");
00411 EXPECT_EQ(jmn[2],"joint_c");
00412 jmn = g_two->getJointModelNames();
00413 std::sort(jmn.begin(), jmn.end());
00414 EXPECT_EQ(jmn[0],"base_joint");
00415 EXPECT_EQ(jmn[1],"joint_a");
00416 EXPECT_EQ(jmn[2],"joint_b");
00417 jmn = g_three->getJointModelNames();
00418 std::sort(jmn.begin(), jmn.end());
00419 EXPECT_EQ(jmn[0],"base_joint");
00420 EXPECT_EQ(jmn[1],"joint_a");
00421 EXPECT_EQ(jmn[2],"joint_b");
00422 EXPECT_EQ(jmn[3],"joint_c");
00423
00424
00425 ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 6);
00426 ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 6);
00427 ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 6);
00428
00429 EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(),"base_link");
00430 EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(),"link_a");
00431 EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(),"link_b");
00432 EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(),"link_c");
00433
00434 EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(),"base_link");
00435 EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(),"link_a");
00436 EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(),"link_b");
00437 EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(),"link_c");
00438
00439 EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(),"base_link");
00440 EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(),"link_a");
00441 EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(),"link_b");
00442 EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(),"link_c");
00443
00444
00445
00446 moveit::core::RobotState state(model);
00447
00448 EXPECT_EQ((unsigned int)7, state.getVariableCount());
00449
00450 state.setToDefaultValues();
00451
00452 std::map<std::string, double> joint_values;
00453 joint_values["base_joint/x"]=1.0;
00454 joint_values["base_joint/y"]=1.0;
00455 joint_values["base_joint/theta"]=0.5;
00456 joint_values["joint_a"] = -0.5;
00457 joint_values["joint_c"] = 0.08;
00458 state.setVariablePositions(joint_values);
00459
00460 EXPECT_NEAR(1.0, state.getGlobalLinkTransform("base_link").translation().x(), 1e-5);
00461 EXPECT_NEAR(1.0, state.getGlobalLinkTransform("base_link").translation().y(), 1e-5);
00462 EXPECT_NEAR(0.0, state.getGlobalLinkTransform("base_link").translation().z(), 1e-5);
00463 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5);
00464 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5);
00465 EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5);
00466 EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5);
00467
00468 EXPECT_NEAR(1.0, state.getGlobalLinkTransform("link_a").translation().x(), 1e-5);
00469 EXPECT_NEAR(1.0, state.getGlobalLinkTransform("link_a").translation().y(), 1e-5);
00470 EXPECT_NEAR(0.0, state.getGlobalLinkTransform("link_a").translation().z(), 1e-5);
00471 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5);
00472 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5);
00473 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5);
00474 EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5);
00475
00476 EXPECT_NEAR(1.0, state.getGlobalLinkTransform("link_b").translation().x(), 1e-5);
00477 EXPECT_NEAR(1.5, state.getGlobalLinkTransform("link_b").translation().y(), 1e-5);
00478 EXPECT_NEAR(0.0, state.getGlobalLinkTransform("link_b").translation().z(), 1e-5);
00479 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5);
00480 EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5);
00481 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5);
00482 EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5);
00483
00484 EXPECT_NEAR(1.08, state.getGlobalLinkTransform("link_c").translation().x(), 1e-5);
00485 EXPECT_NEAR(1.4, state.getGlobalLinkTransform("link_c").translation().y(), 1e-5);
00486 EXPECT_NEAR(0.0, state.getGlobalLinkTransform("link_c").translation().z(), 1e-5);
00487 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5);
00488 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5);
00489 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5);
00490 EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5);
00491
00492 EXPECT_TRUE(state.satisfiesBounds());
00493
00494 std::map<std::string, double> upd_a;
00495 upd_a["joint_a"] = 0.2;
00496 state.setVariablePositions(upd_a);
00497 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
00498 EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
00499 state.enforceBounds();
00500 EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
00501
00502 upd_a["joint_a"] = 3.2;
00503 state.setVariablePositions(upd_a);
00504 EXPECT_FALSE(state.satisfiesBounds(model->getJointModel("joint_a")));
00505 EXPECT_NEAR(state.getVariablePosition("joint_a"), 3.2, 1e-3);
00506 state.enforceBounds();
00507 EXPECT_NEAR(state.getVariablePosition("joint_a"), -3.083185, 1e-3);
00508 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
00509 }
00510
00511 int main(int argc, char **argv)
00512 {
00513 testing::InitGoogleTest(&argc, argv);
00514 return RUN_ALL_TESTS();
00515 }