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