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
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
00094 {
00095 planning_models::KinematicState state(model);
00096
00097 state.setKinematicStateToDefault();
00098
00099
00100 planning_models::KinematicState new_state(state);
00101
00102
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
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
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
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
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
00363 gcs.push_back(planning_models::KinematicModel::GroupConfig("base_from_base_to_tip","odom_combined","link_b"));
00364
00365
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
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
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
00395 ASSERT_EQ(g_one->getGroupLinkModels().size(), 4);
00396
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
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
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
00446
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
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
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 }