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/test_resources/config.h>
00038 #include <moveit/kinematic_constraints/kinematic_constraint.h>
00039 #include <gtest/gtest.h>
00040 #include <urdf_parser/urdf_parser.h>
00041 #include <fstream>
00042 #include <eigen_conversions/eigen_msg.h>
00043 #include <boost/filesystem/path.hpp>
00044
00045 class LoadPlanningModelsPr2 : public testing::Test
00046 {
00047 protected:
00048
00049 virtual void SetUp()
00050 {
00051 srdf_model.reset(new srdf::Model());
00052 std::string xml_string;
00053 std::fstream xml_file((boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "urdf/robot.xml").string().c_str(), std::fstream::in);
00054 if (xml_file.is_open())
00055 {
00056 while ( xml_file.good() )
00057 {
00058 std::string line;
00059 std::getline( xml_file, line);
00060 xml_string += (line + "\n");
00061 }
00062 xml_file.close();
00063 urdf_model = urdf::parseURDF(xml_string);
00064 }
00065 srdf_model->initFile(*urdf_model, (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "srdf/robot.xml").string());
00066 kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
00067 };
00068
00069 virtual void TearDown()
00070 {
00071 }
00072
00073 protected:
00074
00075 boost::shared_ptr<urdf::ModelInterface> urdf_model;
00076 boost::shared_ptr<srdf::Model> srdf_model;
00077 robot_model::RobotModelPtr kmodel;
00078 };
00079
00080 TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
00081 {
00082 robot_state::RobotState ks(kmodel);
00083 ks.setToDefaultValues();
00084 robot_state::Transforms tf(kmodel->getModelFrame());
00085
00086 kinematic_constraints::JointConstraint jc(kmodel);
00087 moveit_msgs::JointConstraint jcm;
00088 jcm.joint_name = "head_pan_joint";
00089 jcm.position = 0.4;
00090 jcm.tolerance_above = 0.1;
00091 jcm.tolerance_below = 0.05;
00092
00093 EXPECT_TRUE(jc.configure(jcm));
00094
00095 EXPECT_NEAR(jc.getConstraintWeight(), 1.0, std::numeric_limits<double>::epsilon());
00096
00097
00098
00099 EXPECT_TRUE(jc.configure(jcm));
00100 kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
00101 EXPECT_FALSE(p1.satisfied);
00102 EXPECT_NEAR(p1.distance, jcm.position, 1e-6);
00103
00104
00105
00106 std::map<std::string, double> jvals;
00107 jvals[jcm.joint_name] = 0.41;
00108 ks.setStateValues(jvals);
00109 kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
00110 EXPECT_TRUE(p2.satisfied);
00111 EXPECT_NEAR(p2.distance, 0.01, 1e-6);
00112
00113
00114 jvals[jcm.joint_name] = 0.35;
00115 ks.setStateValues(jvals);
00116 EXPECT_TRUE(jc.decide(ks).satisfied);
00117
00118
00119 jvals[jcm.joint_name] = 0.35-std::numeric_limits<double>::epsilon();
00120 ks.setStateValues(jvals);
00121 EXPECT_TRUE(jc.decide(ks).satisfied);
00122
00123
00124 jvals[jcm.joint_name] = 0.35-3*std::numeric_limits<double>::epsilon();
00125 ks.setStateValues(jvals);
00126 EXPECT_FALSE(jc.decide(ks).satisfied);
00127
00128
00129 jcm.tolerance_below = -0.05;
00130 EXPECT_FALSE(jc.configure(jcm));
00131
00132 jcm.tolerance_below = 0.05;
00133 EXPECT_TRUE(jc.configure(jcm));
00134
00135
00136 jvals[jcm.joint_name] = 0.46;
00137 ks.setStateValues(jvals);
00138 EXPECT_TRUE(jc.decide(ks).satisfied);
00139
00140
00141 jvals[jcm.joint_name] = 0.501;
00142 ks.setStateValues(jvals);
00143 EXPECT_FALSE(jc.decide(ks).satisfied);
00144
00145
00146 jvals[jcm.joint_name] = 0.39;
00147 ks.setStateValues(jvals);
00148 EXPECT_TRUE(jc.decide(ks).satisfied);
00149
00150
00151 jvals[jcm.joint_name] = 0.34;
00152 ks.setStateValues(jvals);
00153 EXPECT_FALSE(jc.decide(ks).satisfied);
00154
00155
00156 kinematic_constraints::JointConstraint jc2(kmodel);
00157 EXPECT_TRUE(jc2.configure(jcm));
00158 EXPECT_TRUE(jc2.enabled());
00159 EXPECT_TRUE(jc.equal(jc2, 1e-12));
00160
00161
00162 jcm.joint_name = "head_tilt_joint";
00163 EXPECT_TRUE(jc2.configure(jcm));
00164 EXPECT_FALSE(jc.equal(jc2, 1e-12));
00165
00166
00167 jcm.joint_name = "head_pan_joint";
00168 jcm.position = 0.3;
00169 EXPECT_TRUE(jc2.configure(jcm));
00170 EXPECT_FALSE(jc.equal(jc2, 1e-12));
00171
00172 EXPECT_FALSE(jc.equal(jc2, .1));
00173 EXPECT_TRUE(jc.equal(jc2, .101));
00174
00175
00176 jcm.joint_name = "";
00177 jcm.position = 0.4;
00178 EXPECT_FALSE(jc2.configure(jcm));
00179 EXPECT_FALSE(jc2.enabled());
00180 EXPECT_FALSE(jc.equal(jc2, 1e-12));
00181
00182
00183 jcm.joint_name = "base_footprint_joint";
00184 EXPECT_FALSE(jc2.configure(jcm));
00185
00186
00187 jcm.joint_name = "head_pan_joint";
00188 EXPECT_TRUE(jc2.configure(jcm));
00189 jc2.clear();
00190 EXPECT_FALSE(jc2.enabled());
00191 EXPECT_FALSE(jc.equal(jc2, 1e-12));
00192
00193 }
00194
00195 TEST_F(LoadPlanningModelsPr2, JointConstraintsCont)
00196 {
00197 robot_state::RobotState ks(kmodel);
00198 ks.setToDefaultValues();
00199 robot_state::Transforms tf(kmodel->getModelFrame());
00200
00201 kinematic_constraints::JointConstraint jc(kmodel);
00202 moveit_msgs::JointConstraint jcm;
00203
00204 jcm.joint_name = "l_wrist_roll_joint";
00205 jcm.position = 0.0;
00206 jcm.tolerance_above = 0.04;
00207 jcm.tolerance_below = 0.02;
00208 jcm.weight = 1.0;
00209
00210 EXPECT_TRUE(jc.configure(jcm));
00211
00212 std::map<std::string, double> jvals;
00213
00214
00215 EXPECT_TRUE(jc.decide(ks).satisfied);
00216
00217
00218 jvals[jcm.joint_name] = .03;
00219 ks.setStateValues(jvals);
00220 EXPECT_TRUE(jc.decide(ks).satisfied);
00221
00222
00223 jvals[jcm.joint_name] = .05;
00224 ks.setStateValues(jvals);
00225 EXPECT_FALSE(jc.decide(ks).satisfied);
00226
00227
00228 jvals[jcm.joint_name] = -.01;
00229 ks.setStateValues(jvals);
00230 EXPECT_TRUE(jc.decide(ks).satisfied);
00231
00232
00233 jvals[jcm.joint_name] = -.03;
00234 ks.setStateValues(jvals);
00235 EXPECT_FALSE(jc.decide(ks).satisfied);
00236
00237
00238 jcm.position = 3.14;
00239 EXPECT_TRUE(jc.configure(jcm));
00240
00241
00242 jvals[jcm.joint_name] = 3.17;
00243 ks.setStateValues(jvals);
00244 kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
00245 EXPECT_TRUE(p1.satisfied);
00246 EXPECT_NEAR(p1.distance, 0.03, 1e-6);
00247
00248
00249 jvals[jcm.joint_name] = -3.14;
00250 ks.setStateValues(jvals);
00251 kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
00252 EXPECT_TRUE(p2.satisfied);
00253 EXPECT_NEAR(p2.distance, 0.003185, 1e-4);
00254
00255
00256 jvals[jcm.joint_name] = 3.19;
00257 ks.setStateValues(jvals);
00258 EXPECT_FALSE(jc.decide(ks).satisfied);
00259
00260
00261
00262 jvals[jcm.joint_name] = -3.11;
00263 ks.setStateValues(jvals);
00264 EXPECT_TRUE(jc.decide(ks).satisfied);
00265
00266
00267 jvals[jcm.joint_name] = -3.09;
00268 ks.setStateValues(jvals);
00269 EXPECT_FALSE(jc.decide(ks).satisfied);
00270
00271
00272
00273 jvals[jcm.joint_name] = 3.13;
00274 ks.setStateValues(jvals);
00275 EXPECT_TRUE(jc.decide(ks).satisfied);
00276
00277
00278 jvals[jcm.joint_name] = 3.11;
00279 ks.setStateValues(jvals);
00280 EXPECT_FALSE(jc.decide(ks).satisfied);
00281
00282
00283 jcm.position = -3.14;
00284 EXPECT_TRUE(jc.configure(jcm));
00285
00286
00287 jvals[jcm.joint_name] = -3.11;
00288 ks.setStateValues(jvals);
00289 EXPECT_TRUE(jc.decide(ks).satisfied);
00290
00291
00292 jvals[jcm.joint_name] = -3.09;
00293 ks.setStateValues(jvals);
00294 EXPECT_FALSE(jc.decide(ks).satisfied);
00295
00296
00297 jvals[jcm.joint_name] = 3.13;
00298 ks.setStateValues(jvals);
00299 EXPECT_TRUE(jc.decide(ks).satisfied);
00300
00301
00302 jvals[jcm.joint_name] = 3.12;
00303 ks.setStateValues(jvals);
00304 EXPECT_TRUE(jc.decide(ks).satisfied);
00305
00306
00307 jcm.position = 6.28;
00308 EXPECT_TRUE(jc.configure(jcm));
00309
00310
00311 jvals[jcm.joint_name] = 0.0;
00312 ks.setStateValues(jvals);
00313 EXPECT_TRUE(jc.decide(ks).satisfied);
00314
00315
00316 moveit_msgs::JointConstraint jcm2 = jcm;
00317 jcm2.position = -6.28;
00318 kinematic_constraints::JointConstraint jc2(kmodel);
00319 EXPECT_TRUE(jc.configure(jcm2));
00320 jc.equal(jc2, .02);
00321
00322 }
00323
00324 TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF)
00325 {
00326 robot_state::RobotState ks(kmodel);
00327 ks.setToDefaultValues();
00328
00329 kinematic_constraints::JointConstraint jc(kmodel);
00330 moveit_msgs::JointConstraint jcm;
00331 jcm.joint_name = "world_joint";
00332 jcm.position = 3.14;
00333 jcm.tolerance_above = 0.1;
00334 jcm.tolerance_below = 0.05;
00335 jcm.weight = 1.0;
00336
00337
00338 EXPECT_FALSE(jc.configure(jcm));
00339
00340
00341 jcm.joint_name = "world_joint/x";
00342 EXPECT_TRUE(jc.configure(jcm));
00343
00344 std::map<std::string, double> jvals;
00345 jvals[jcm.joint_name] = 3.2;
00346 ks.setStateValues(jvals);
00347 kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
00348 EXPECT_TRUE(p1.satisfied);
00349
00350 jvals[jcm.joint_name] = 3.25;
00351 ks.setStateValues(jvals);
00352 kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
00353 EXPECT_FALSE(p2.satisfied);
00354
00355 jvals[jcm.joint_name] = -3.14;
00356 ks.setStateValues(jvals);
00357 kinematic_constraints::ConstraintEvaluationResult p3 = jc.decide(ks);
00358 EXPECT_FALSE(p3.satisfied);
00359
00360
00361 jcm.joint_name = "world_joint/theta";
00362 EXPECT_TRUE(jc.configure(jcm));
00363
00364 jvals[jcm.joint_name] = -3.14;
00365 ks.setStateValues(jvals);
00366 kinematic_constraints::ConstraintEvaluationResult p4 = jc.decide(ks);
00367 EXPECT_TRUE(p4.satisfied);
00368
00369 jvals[jcm.joint_name] = 3.25;
00370 ks.setStateValues(jvals);
00371 kinematic_constraints::ConstraintEvaluationResult p5 = jc.decide(ks);
00372 EXPECT_FALSE(p5.satisfied);
00373 }
00374
00375 TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed)
00376 {
00377 robot_state::RobotState ks(kmodel);
00378 ks.setToDefaultValues();
00379 robot_state::Transforms tf(kmodel->getModelFrame());
00380
00381 kinematic_constraints::PositionConstraint pc(kmodel);
00382 moveit_msgs::PositionConstraint pcm;
00383
00384
00385 EXPECT_FALSE(pc.configure(pcm, tf));
00386
00387 pcm.link_name = "l_wrist_roll_link";
00388 pcm.target_point_offset.x = 0;
00389 pcm.target_point_offset.y = 0;
00390 pcm.target_point_offset.z = 0;
00391 pcm.constraint_region.primitives.resize(1);
00392 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00393
00394
00395 EXPECT_FALSE(pc.configure(pcm, tf));
00396
00397 pcm.constraint_region.primitives[0].dimensions.resize(1);
00398 pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
00399
00400
00401 EXPECT_FALSE(pc.configure(pcm, tf));
00402
00403 pcm.constraint_region.primitive_poses.resize(1);
00404 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00405 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00406 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00407 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00408 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00409 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00410 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00411 pcm.weight = 1.0;
00412
00413
00414 EXPECT_FALSE(pc.configure(pcm, tf));
00415
00416 pcm.header.frame_id = kmodel->getModelFrame();
00417 EXPECT_TRUE(pc.configure(pcm, tf));
00418 EXPECT_FALSE(pc.mobileReferenceFrame());
00419
00420 EXPECT_TRUE(pc.decide(ks).satisfied);
00421
00422 std::map<std::string, double> jvals;
00423 jvals["torso_lift_joint"] = 0.4;
00424 ks.setStateValues(jvals);
00425 EXPECT_FALSE(pc.decide(ks).satisfied);
00426 EXPECT_TRUE(pc.equal(pc, 1e-12));
00427
00428
00429 pcm.target_point_offset.x = 0;
00430 pcm.target_point_offset.y = 0;
00431 pcm.target_point_offset.z = .15;
00432
00433 EXPECT_TRUE(pc.configure(pcm, tf));
00434 EXPECT_TRUE(pc.hasLinkOffset());
00435 EXPECT_TRUE(pc.decide(ks).satisfied);
00436
00437 pc.clear();
00438 EXPECT_FALSE(pc.enabled());
00439
00440
00441 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00442 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00443 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00444 pcm.constraint_region.primitive_poses[0].orientation.w = 0.0;
00445
00446 EXPECT_TRUE(pc.configure(pcm, tf));
00447 EXPECT_TRUE(pc.decide(ks).satisfied);
00448 }
00449
00450 TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile)
00451 {
00452 robot_state::RobotState ks(kmodel);
00453 ks.setToDefaultValues();
00454 robot_state::Transforms tf(kmodel->getModelFrame());
00455
00456 kinematic_constraints::PositionConstraint pc(kmodel);
00457 moveit_msgs::PositionConstraint pcm;
00458
00459 pcm.link_name = "l_wrist_roll_link";
00460 pcm.target_point_offset.x = 0;
00461 pcm.target_point_offset.y = 0;
00462 pcm.target_point_offset.z = 0;
00463
00464 pcm.constraint_region.primitives.resize(1);
00465 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00466 pcm.constraint_region.primitives[0].dimensions.resize(1);
00467 pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.38 * 2.0;
00468
00469 pcm.header.frame_id = "r_wrist_roll_link";
00470
00471 pcm.constraint_region.primitive_poses.resize(1);
00472 pcm.constraint_region.primitive_poses[0].position.x = 0.0;
00473 pcm.constraint_region.primitive_poses[0].position.y = 0.6;
00474 pcm.constraint_region.primitive_poses[0].position.z = 0.0;
00475 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00476 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00477 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00478 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00479 pcm.weight = 1.0;
00480
00481 EXPECT_FALSE(tf.isFixedFrame(pcm.link_name));
00482 EXPECT_TRUE(pc.configure(pcm, tf));
00483 EXPECT_TRUE(pc.mobileReferenceFrame());
00484
00485 EXPECT_TRUE(pc.decide(ks).satisfied);
00486
00487 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
00488 pcm.constraint_region.primitives[0].dimensions.resize(3);
00489 pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1;
00490 pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
00491 pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.1;
00492 EXPECT_TRUE(pc.configure(pcm, tf));
00493
00494 std::map<std::string, double> jvals;
00495 jvals["l_shoulder_pan_joint"] = 0.4;
00496 ks.setStateValues(jvals);
00497 EXPECT_TRUE(pc.decide(ks).satisfied);
00498 EXPECT_TRUE(pc.equal(pc, 1e-12));
00499
00500 jvals["l_shoulder_pan_joint"] = -0.4;
00501 ks.setStateValues(jvals);
00502 EXPECT_FALSE(pc.decide(ks).satisfied);
00503
00504
00505 pcm.constraint_region.primitive_poses.resize(2);
00506 pcm.constraint_region.primitive_poses[1].position.x = 0.0;
00507 pcm.constraint_region.primitive_poses[1].position.y = 0.1;
00508 pcm.constraint_region.primitive_poses[1].position.z = 0.0;
00509 pcm.constraint_region.primitive_poses[1].orientation.x = 0.0;
00510 pcm.constraint_region.primitive_poses[1].orientation.y = 0.0;
00511 pcm.constraint_region.primitive_poses[1].orientation.z = 0.0;
00512 pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
00513
00514 pcm.constraint_region.primitives.resize(2);
00515 pcm.constraint_region.primitives[1].type = shape_msgs::SolidPrimitive::BOX;
00516 pcm.constraint_region.primitives[1].dimensions.resize(3);
00517 pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1;
00518 pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
00519 pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.1;
00520 EXPECT_TRUE(pc.configure(pcm, tf));
00521 EXPECT_TRUE(pc.decide(ks, false).satisfied);
00522 }
00523
00524 TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality)
00525 {
00526 robot_state::RobotState ks(kmodel);
00527 ks.setToDefaultValues();
00528 robot_state::Transforms tf(kmodel->getModelFrame());
00529
00530 kinematic_constraints::PositionConstraint pc(kmodel);
00531 kinematic_constraints::PositionConstraint pc2(kmodel);
00532 moveit_msgs::PositionConstraint pcm;
00533
00534 pcm.link_name = "l_wrist_roll_link";
00535 pcm.target_point_offset.x = 0;
00536 pcm.target_point_offset.y = 0;
00537 pcm.target_point_offset.z = 0;
00538
00539 pcm.constraint_region.primitives.resize(2);
00540 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00541 pcm.constraint_region.primitives[0].dimensions.resize(1);
00542 pcm.constraint_region.primitives[0].dimensions[0] = 0.38 * 2.0;
00543 pcm.constraint_region.primitives[1].type = shape_msgs::SolidPrimitive::BOX;
00544 pcm.constraint_region.primitives[1].dimensions.resize(3);
00545 pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.0;
00546 pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 2.0;
00547 pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 2.0;
00548
00549 pcm.header.frame_id = "r_wrist_roll_link";
00550 pcm.constraint_region.primitive_poses.resize(2);
00551 pcm.constraint_region.primitive_poses[0].position.x = 0.0;
00552 pcm.constraint_region.primitive_poses[0].position.y = 0.6;
00553 pcm.constraint_region.primitive_poses[0].position.z = 0.0;
00554 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00555 pcm.constraint_region.primitive_poses[1].position.x = 2.0;
00556 pcm.constraint_region.primitive_poses[1].position.y = 0.0;
00557 pcm.constraint_region.primitive_poses[1].position.z = 0.0;
00558 pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
00559 pcm.weight = 1.0;
00560
00561 EXPECT_TRUE(pc.configure(pcm, tf));
00562 EXPECT_TRUE(pc2.configure(pcm, tf));
00563
00564 EXPECT_TRUE(pc.equal(pc2, .001));
00565 EXPECT_TRUE(pc2.equal(pc, .001));
00566
00567
00568 moveit_msgs::PositionConstraint pcm2 = pcm;
00569 pcm2.constraint_region.primitives[0] = pcm.constraint_region.primitives[1];
00570 pcm2.constraint_region.primitives[1] = pcm.constraint_region.primitives[0];
00571
00572 pcm2.constraint_region.primitive_poses[0] = pcm.constraint_region.primitive_poses[1];
00573 pcm2.constraint_region.primitive_poses[1] = pcm.constraint_region.primitive_poses[0];
00574
00575 EXPECT_TRUE(pc2.configure(pcm2, tf));
00576 EXPECT_TRUE(pc.equal(pc2, .001));
00577 EXPECT_TRUE(pc2.equal(pc, .001));
00578
00579
00580 pcm2.constraint_region.primitive_poses[0].position.z = .01;
00581 EXPECT_TRUE(pc2.configure(pcm2, tf));
00582 EXPECT_FALSE(pc.equal(pc2, .001));
00583 EXPECT_FALSE(pc2.equal(pc, .001));
00584 EXPECT_TRUE(pc.equal(pc2, .1));
00585 EXPECT_TRUE(pc2.equal(pc, .1));
00586
00587
00588 pcm2.constraint_region.primitive_poses[0].position.z = 0.0;
00589 pcm2.constraint_region.primitives.resize(3);
00590 pcm2.constraint_region.primitive_poses.resize(3);
00591 pcm2.constraint_region.primitives[2] = pcm2.constraint_region.primitives[0];
00592 pcm2.constraint_region.primitive_poses[2] = pcm2.constraint_region.primitive_poses[0];
00593 EXPECT_TRUE(pc2.configure(pcm2, tf));
00594 EXPECT_TRUE(pc.equal(pc2, .001));
00595 EXPECT_TRUE(pc2.equal(pc, .001));
00596
00597
00598 pcm2.constraint_region.primitives[2].dimensions[0] = 3.0;
00599 EXPECT_TRUE(pc2.configure(pcm2, tf));
00600 EXPECT_FALSE(pc.equal(pc2, .001));
00601 EXPECT_FALSE(pc2.equal(pc, .001));
00602
00603
00604 pcm2.constraint_region.primitives[2].dimensions[0] = pcm2.constraint_region.primitives[0].dimensions[0];
00605 pcm2.constraint_region.primitives[2].type = shape_msgs::SolidPrimitive::SPHERE;
00606 EXPECT_TRUE(pc2.configure(pcm2, tf));
00607 EXPECT_FALSE(pc.equal(pc2, .001));
00608 }
00609
00610
00611 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
00612 {
00613 robot_state::RobotState ks(kmodel);
00614 ks.setToDefaultValues();
00615 robot_state::Transforms tf(kmodel->getModelFrame());
00616
00617 kinematic_constraints::OrientationConstraint oc(kmodel);
00618 moveit_msgs::OrientationConstraint ocm;
00619
00620 EXPECT_FALSE(oc.configure(ocm, tf));
00621
00622 ocm.link_name = "r_wrist_roll_link";
00623
00624
00625 EXPECT_TRUE(oc.configure(ocm, tf));
00626
00627 ocm.header.frame_id = kmodel->getModelFrame();
00628 ocm.orientation.x = 0.0;
00629 ocm.orientation.y = 0.0;
00630 ocm.orientation.z = 0.0;
00631 ocm.orientation.w = 1.0;
00632 ocm.absolute_x_axis_tolerance = 0.1;
00633 ocm.absolute_y_axis_tolerance = 0.1;
00634 ocm.absolute_z_axis_tolerance = 0.1;
00635 ocm.weight = 1.0;
00636
00637 EXPECT_TRUE(oc.configure(ocm, tf));
00638 EXPECT_FALSE(oc.mobileReferenceFrame());
00639
00640 EXPECT_FALSE(oc.decide(ks).satisfied);
00641
00642 ocm.header.frame_id = ocm.link_name;
00643 EXPECT_TRUE(oc.configure(ocm, tf));
00644 EXPECT_TRUE(oc.decide(ks).satisfied);
00645 EXPECT_TRUE(oc.equal(oc, 1e-12));
00646 EXPECT_TRUE(oc.mobileReferenceFrame());
00647
00648 ASSERT_TRUE(oc.getLinkModel());
00649
00650 geometry_msgs::Pose p;
00651 tf::poseEigenToMsg(ks.getLinkState(oc.getLinkModel()->getName())->getGlobalLinkTransform(), p);
00652
00653 ocm.orientation = p.orientation;
00654 ocm.header.frame_id = kmodel->getModelFrame();
00655 EXPECT_TRUE(oc.configure(ocm, tf));
00656 EXPECT_TRUE(oc.decide(ks).satisfied);
00657
00658 std::map<std::string, double> jvals;
00659 jvals["r_wrist_roll_joint"] = .05;
00660 ks.setStateValues(jvals);
00661 EXPECT_TRUE(oc.decide(ks).satisfied);
00662
00663 jvals["r_wrist_roll_joint"] = .11;
00664 ks.setStateValues(jvals);
00665 EXPECT_FALSE(oc.decide(ks).satisfied);
00666 }
00667
00668 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple)
00669 {
00670 robot_state::RobotState ks(kmodel);
00671 ks.setToDefaultValues();
00672 robot_state::Transforms tf(kmodel->getModelFrame());
00673
00674 kinematic_constraints::VisibilityConstraint vc(kmodel);
00675 moveit_msgs::VisibilityConstraint vcm;
00676
00677 EXPECT_FALSE(vc.configure(vcm, tf));
00678
00679 vcm.sensor_pose.header.frame_id = "base_footprint";
00680 vcm.sensor_pose.pose.position.z = -1.0;
00681 vcm.sensor_pose.pose.orientation.y = 1.0;
00682
00683 vcm.target_pose.header.frame_id = "base_footprint";
00684 vcm.target_pose.pose.position.z = -2.0;
00685 vcm.target_pose.pose.orientation.y = 0.0;
00686 vcm.target_pose.pose.orientation.w = 1.0;
00687
00688 vcm.target_radius = .2;
00689 vcm.cone_sides = 10;
00690 vcm.max_view_angle = 0.0;
00691 vcm.max_range_angle = 0.0;
00692 vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_Z;
00693 vcm.weight = 1.0;
00694
00695 EXPECT_TRUE(vc.configure(vcm, tf));
00696
00697 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00698
00699 vcm.max_view_angle = .1;
00700
00701
00702 EXPECT_TRUE(vc.configure(vcm, tf));
00703 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00704
00705
00706 vcm.target_pose.pose.orientation.y = 0.03;
00707 vcm.target_pose.pose.orientation.w = .9995;
00708 EXPECT_TRUE(vc.configure(vcm, tf));
00709 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00710
00711
00712 vcm.target_pose.pose.orientation.y = 0.06;
00713 vcm.target_pose.pose.orientation.w = .9981;
00714 EXPECT_TRUE(vc.configure(vcm, tf));
00715 EXPECT_FALSE(vc.decide(ks, true).satisfied);
00716 }
00717
00718 TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2)
00719 {
00720
00721 robot_state::RobotState ks(kmodel);
00722 ks.setToDefaultValues();
00723 robot_state::Transforms tf(kmodel->getModelFrame());
00724
00725 kinematic_constraints::VisibilityConstraint vc(kmodel);
00726 moveit_msgs::VisibilityConstraint vcm;
00727
00728 vcm.sensor_pose.header.frame_id = "narrow_stereo_optical_frame";
00729 vcm.sensor_pose.pose.position.z = 0.05;
00730 vcm.sensor_pose.pose.orientation.w = 1.0;
00731
00732 vcm.target_pose.header.frame_id = "l_gripper_r_finger_tip_link";
00733 vcm.target_pose.pose.position.z = 0.03;
00734 vcm.target_pose.pose.orientation.w = 1.0;
00735
00736 vcm.cone_sides = 10;
00737 vcm.max_view_angle = 0.0;
00738 vcm.max_range_angle = 0.0;
00739 vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_Z;
00740 vcm.weight = 1.0;
00741
00742
00743 EXPECT_FALSE(vc.configure(vcm, tf));
00744
00745
00746
00747 vcm.target_radius = .05;
00748 EXPECT_TRUE(vc.configure(vcm, tf));
00749 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00750
00751
00752
00753 std::map<std::string, double> state_values;
00754 state_values["l_shoulder_lift_joint"] = .5;
00755 state_values["r_shoulder_pan_joint"] = .5;
00756 state_values["r_elbow_flex_joint"] = -1.4;
00757 ks.setStateValues(state_values);
00758 EXPECT_FALSE(vc.decide(ks, true).satisfied);
00759
00760
00761 state_values["r_shoulder_pan_joint"] = .4;
00762 ks.setStateValues(state_values);
00763 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00764
00765
00766 state_values["l_shoulder_lift_joint"] = 0;
00767 state_values["r_shoulder_pan_joint"] = .5;
00768 state_values["r_elbow_flex_joint"] = -.6;
00769 ks.setStateValues(state_values);
00770 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00771
00772
00773 vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
00774 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00775
00776 ks.setToDefaultValues();
00777
00778
00779 vcm.target_radius = .01;
00780 vcm.target_pose.pose.position.z = 0.00;
00781 vcm.target_pose.pose.position.x = 0.035;
00782 EXPECT_TRUE(vc.configure(vcm, tf));
00783 EXPECT_TRUE(vc.decide(ks, true).satisfied);
00784
00785
00786 vcm.target_radius = .05;
00787 EXPECT_TRUE(vc.configure(vcm, tf));
00788 EXPECT_FALSE(vc.decide(ks, true).satisfied);
00789 }
00790
00791 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet)
00792 {
00793 robot_state::RobotState ks(kmodel);
00794 ks.setToDefaultValues();
00795 robot_state::Transforms tf(kmodel->getModelFrame());
00796
00797 kinematic_constraints::KinematicConstraintSet kcs(kmodel);
00798 EXPECT_TRUE(kcs.empty());
00799
00800 moveit_msgs::JointConstraint jcm;
00801 jcm.joint_name = "head_pan_joint";
00802 jcm.position = 0.4;
00803 jcm.tolerance_above = 0.1;
00804 jcm.tolerance_below = 0.05;
00805 jcm.weight = 1.0;
00806
00807
00808 std::vector<moveit_msgs::JointConstraint> jcv;
00809 jcv.push_back(jcm);
00810 EXPECT_TRUE(kcs.add(jcv));
00811
00812
00813 EXPECT_FALSE(kcs.decide(ks).satisfied);
00814
00815
00816 std::map<std::string, double> jvals;
00817 jvals[jcm.joint_name] = 0.41;
00818 ks.setStateValues(jvals);
00819 EXPECT_TRUE(kcs.decide(ks).satisfied);
00820
00821
00822 EXPECT_FALSE(kcs.empty());
00823 kcs.clear();
00824 EXPECT_TRUE(kcs.empty());
00825 jcv.push_back(jcm);
00826 jcv.back().joint_name = "head_tilt_joint";
00827 EXPECT_TRUE(kcs.add(jcv));
00828
00829
00830 EXPECT_FALSE(kcs.decide(ks).satisfied);
00831
00832
00833 jvals[jcv.back().joint_name] = 0.41;
00834 ks.setStateValues(jvals);
00835 EXPECT_TRUE(kcs.decide(ks).satisfied);
00836
00837
00838 jvals[jcv.back().joint_name] = 0.51;
00839 ks.setStateValues(jvals);
00840 EXPECT_FALSE(kcs.decide(ks).satisfied);
00841
00842
00843 kcs.clear();
00844 jcv.back().joint_name = "no_joint";
00845 EXPECT_FALSE(kcs.add(jcv));
00846
00847
00848 EXPECT_TRUE(kcs.decide(ks).satisfied);
00849
00850
00851 jvals["head_pan_joint"] = 0.51;
00852 ks.setStateValues(jvals);
00853 EXPECT_FALSE(kcs.decide(ks).satisfied);
00854 }
00855
00856 TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality)
00857 {
00858 robot_state::RobotState ks(kmodel);
00859 ks.setToDefaultValues();
00860 robot_state::Transforms tf(kmodel->getModelFrame());
00861
00862 kinematic_constraints::KinematicConstraintSet kcs(kmodel);
00863 kinematic_constraints::KinematicConstraintSet kcs2(kmodel);
00864
00865 moveit_msgs::JointConstraint jcm;
00866 jcm.joint_name = "head_pan_joint";
00867 jcm.position = 0.4;
00868 jcm.tolerance_above = 0.1;
00869 jcm.tolerance_below = 0.05;
00870 jcm.weight = 1.0;
00871
00872 moveit_msgs::PositionConstraint pcm;
00873 pcm.link_name = "l_wrist_roll_link";
00874 pcm.target_point_offset.x = 0;
00875 pcm.target_point_offset.y = 0;
00876 pcm.target_point_offset.z = 0;
00877 pcm.constraint_region.primitives.resize(1);
00878 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00879 pcm.constraint_region.primitives[0].dimensions.resize(1);
00880 pcm.constraint_region.primitives[0].dimensions[0] = 0.2;
00881
00882 pcm.constraint_region.primitive_poses.resize(1);
00883 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00884 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00885 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00886 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00887 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00888 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00889 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00890 pcm.weight = 1.0;
00891
00892 pcm.header.frame_id = kmodel->getModelFrame();
00893
00894
00895
00896 std::vector<moveit_msgs::JointConstraint> jcv;
00897 jcv.push_back(jcm);
00898 EXPECT_TRUE(kcs.add(jcv));
00899
00900 std::vector<moveit_msgs::PositionConstraint> pcv;
00901 pcv.push_back(pcm);
00902 EXPECT_TRUE(kcs.add(pcv, tf));
00903
00904
00905 EXPECT_TRUE(kcs2.add(pcv, tf));
00906 EXPECT_TRUE(kcs2.add(jcv));
00907
00908
00909 EXPECT_TRUE(kcs.equal(kcs2, .001));
00910 EXPECT_TRUE(kcs2.equal(kcs, .001));
00911
00912
00913 jcv.push_back(jcm);
00914 EXPECT_TRUE(kcs2.add(jcv));
00915
00916 EXPECT_TRUE(kcs.equal(kcs2, .001));
00917 EXPECT_TRUE(kcs2.equal(kcs, .001));
00918
00919 jcm.joint_name = "head_pan_joint";
00920 jcm.position = 0.35;
00921 jcm.tolerance_above = 0.1;
00922 jcm.tolerance_below = 0.05;
00923 jcm.weight = 1.0;
00924
00925 jcv.push_back(jcm);
00926 EXPECT_TRUE(kcs2.add(jcv));
00927
00928 EXPECT_FALSE(kcs.equal(kcs2, .001));
00929 EXPECT_FALSE(kcs2.equal(kcs, .001));
00930
00931
00932 EXPECT_TRUE(kcs.equal(kcs2, .1));
00933 EXPECT_TRUE(kcs2.equal(kcs, .1));
00934
00935
00936 }
00937
00938 int main(int argc, char **argv)
00939 {
00940 testing::InitGoogleTest(&argc, argv);
00941 return RUN_ALL_TESTS();
00942 }