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/planning_scene/planning_scene.h>
00038 #include <moveit/kinematic_constraints/kinematic_constraint.h>
00039 #include <moveit/constraint_samplers/default_constraint_samplers.h>
00040 #include <moveit/constraint_samplers/union_constraint_sampler.h>
00041 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
00042 #include <moveit/constraint_samplers/constraint_sampler_tools.h>
00043 #include <moveit_msgs/DisplayTrajectory.h>
00044 #include <moveit/robot_state/conversions.h>
00045
00046 #include <geometric_shapes/shape_operations.h>
00047 #include <ros/package.h>
00048 #include <visualization_msgs/MarkerArray.h>
00049
00050 #include <gtest/gtest.h>
00051 #include <urdf_parser/urdf_parser.h>
00052 #include <fstream>
00053 #include <boost/bind.hpp>
00054 #include <boost/filesystem/path.hpp>
00055
00056 #include "pr2_arm_kinematics_plugin.h"
00057
00058 class LoadPlanningModelsPr2 : public testing::Test
00059 {
00060 protected:
00061
00062 boost::shared_ptr<kinematics::KinematicsBase> getKinematicsSolverRightArm(const robot_model::JointModelGroup *jmg)
00063 {
00064 {
00065 return pr2_kinematics_plugin_right_arm_;
00066 }
00067 }
00068
00069 boost::shared_ptr<kinematics::KinematicsBase> getKinematicsSolverLeftArm(const robot_model::JointModelGroup *jmg)
00070 {
00071 {
00072 return pr2_kinematics_plugin_left_arm_;
00073 }
00074 }
00075
00076 virtual void SetUp()
00077 {
00078 std::string resource_dir = ros::package::getPath("moveit_resources");
00079 if(resource_dir == "")
00080 {
00081 FAIL() << "Failed to find package moveit_resources.";
00082 return;
00083 }
00084 boost::filesystem::path res_path(resource_dir);
00085
00086 srdf_model.reset(new srdf::Model());
00087 std::string xml_string;
00088 std::fstream xml_file((res_path / "test/urdf/robot.xml").string().c_str(), std::fstream::in);
00089 if (xml_file.is_open())
00090 {
00091 while ( xml_file.good() )
00092 {
00093 std::string line;
00094 std::getline( xml_file, line);
00095 xml_string += (line + "\n");
00096 }
00097 xml_file.close();
00098 urdf_model = urdf::parseURDF(xml_string);
00099 }
00100 srdf_model->initFile(*urdf_model, (res_path / "test/srdf/robot.xml").string());
00101 kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
00102
00103 pr2_kinematics_plugin_right_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
00104
00105 pr2_kinematics_plugin_right_arm_->setRobotModel(urdf_model);
00106 pr2_kinematics_plugin_right_arm_->initialize("",
00107 "right_arm",
00108 "torso_lift_link",
00109 "r_wrist_roll_link",
00110 .01);
00111
00112 pr2_kinematics_plugin_left_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
00113
00114 pr2_kinematics_plugin_left_arm_->setRobotModel(urdf_model);
00115 pr2_kinematics_plugin_left_arm_->initialize("",
00116 "left_arm",
00117 "torso_lift_link",
00118 "l_wrist_roll_link",
00119 .01);
00120
00121 func_right_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1)));
00122 func_left_arm.reset(new robot_model::SolverAllocatorFn(boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1)));
00123
00124 std::map<std::string, robot_model::SolverAllocatorFn> allocators;
00125 allocators["right_arm"] = *func_right_arm;
00126 allocators["left_arm"] = *func_left_arm;
00127 allocators["whole_body"] = *func_right_arm;
00128 allocators["base"] = *func_left_arm;
00129
00130 kmodel->setKinematicsAllocators(allocators);
00131
00132 ps.reset(new planning_scene::PlanningScene(kmodel));
00133
00134 };
00135
00136 virtual void TearDown()
00137 {
00138 }
00139
00140 protected:
00141
00142 boost::shared_ptr<urdf::ModelInterface> urdf_model;
00143 boost::shared_ptr<srdf::Model> srdf_model;
00144 robot_model::RobotModelPtr kmodel;
00145 planning_scene::PlanningScenePtr ps;
00146 boost::shared_ptr<pr2_arm_kinematics::PR2ArmKinematicsPlugin> pr2_kinematics_plugin_right_arm_;
00147 boost::shared_ptr<pr2_arm_kinematics::PR2ArmKinematicsPlugin> pr2_kinematics_plugin_left_arm_;
00148 boost::shared_ptr<robot_model::SolverAllocatorFn> func_right_arm;
00149 boost::shared_ptr<robot_model::SolverAllocatorFn> func_left_arm;
00150 };
00151
00152 TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
00153 {
00154 robot_state::RobotState ks(kmodel);
00155 ks.setToDefaultValues();
00156
00157 robot_state::RobotState ks_const(kmodel);
00158 ks_const.setToDefaultValues();
00159
00160 kinematic_constraints::JointConstraint jc1(kmodel);
00161 moveit_msgs::JointConstraint jcm1;
00162
00163 jcm1.position = 0.42;
00164 jcm1.tolerance_above = 0.01;
00165 jcm1.tolerance_below = 0.05;
00166 jcm1.weight = 1.0;
00167 EXPECT_FALSE(jc1.configure(jcm1));
00168
00169 std::vector<kinematic_constraints::JointConstraint> js;
00170 js.push_back(jc1);
00171
00172 constraint_samplers::JointConstraintSampler jcs(ps, "right_arm");
00173
00174 EXPECT_FALSE(jcs.configure(js));
00175
00176
00177 jcm1.joint_name = "r_shoulder_pan_joint";
00178 EXPECT_TRUE(jc1.configure(jcm1));
00179 js.push_back(jc1);
00180 EXPECT_TRUE(jcs.configure(js));
00181 EXPECT_EQ(jcs.getConstrainedJointCount(), 1);
00182 EXPECT_EQ(jcs.getUnconstrainedJointCount(), 6);
00183 EXPECT_TRUE(jcs.sample(ks, ks, 1));
00184
00185 for (int t = 0 ; t < 100 ; ++t)
00186 {
00187 EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00188 EXPECT_TRUE(jc1.decide(ks).satisfied);
00189 }
00190
00191
00192 EXPECT_TRUE(jcs.configure(js));
00193 EXPECT_EQ(jcs.getUnconstrainedJointCount(), 6);
00194
00195 kinematic_constraints::JointConstraint jc2(kmodel);
00196
00197 moveit_msgs::JointConstraint jcm2;
00198 jcm2.joint_name = "r_shoulder_pan_joint";
00199 jcm2.position = 0.54;
00200 jcm2.tolerance_above = 0.01;
00201 jcm2.tolerance_below = 0.01;
00202 jcm2.weight = 1.0;
00203 EXPECT_TRUE(jc2.configure(jcm2));
00204 js.push_back(jc2);
00205
00206
00207 EXPECT_FALSE(jcs.configure(js));
00208 EXPECT_FALSE(jcs.sample(ks, ks_const, 1));
00209
00210
00211 constraint_samplers::JointConstraintSampler jcs2(ps, "arms");
00212 jcs2.configure(js);
00213 EXPECT_FALSE(jcs2.sample(ks, ks_const, 1));
00214
00215
00216 constraint_samplers::JointConstraintSampler jcs3(ps, "left_arm");
00217 EXPECT_FALSE(jcs3.configure(js));
00218
00219
00220 js.clear();
00221
00222 jcm1.position = .4;
00223 jcm1.tolerance_above = .05;
00224 jcm1.tolerance_below = .05;
00225 jcm2.position = .4;
00226 jcm2.tolerance_above = .1;
00227 jcm2.tolerance_below = .1;
00228 EXPECT_TRUE(jc1.configure(jcm1));
00229 EXPECT_TRUE(jc2.configure(jcm2));
00230 js.push_back(jc1);
00231 js.push_back(jc2);
00232
00233 EXPECT_TRUE(jcs.configure(js));
00234
00235
00236 for (int t = 0 ; t < 100 ; ++t)
00237 {
00238 EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00239 EXPECT_TRUE(jc1.decide(ks).satisfied);
00240 }
00241
00242
00243 js.clear();
00244
00245 jcm1.position = -3.1;
00246 jcm1.tolerance_above = .05;
00247 jcm1.tolerance_below = .05;
00248
00249
00250 EXPECT_TRUE(jc1.configure(jcm1));
00251 js.push_back(jc1);
00252 EXPECT_TRUE(jcs.configure(js));
00253
00254
00255 js.clear();
00256 jcm1.position = .35;
00257 jcm1.tolerance_above = .05;
00258 jcm1.tolerance_below = .05;
00259 jcm2.position = .45;
00260 jcm2.tolerance_above = .05;
00261 jcm2.tolerance_below = .05;
00262 EXPECT_TRUE(jc1.configure(jcm1));
00263 EXPECT_TRUE(jc2.configure(jcm2));
00264 js.push_back(jc1);
00265 js.push_back(jc2);
00266
00267
00268 EXPECT_TRUE(jcs.configure(js));
00269 for (int t = 0 ; t < 100 ; ++t)
00270 {
00271 EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00272 std::map<std::string, double> var_values;
00273 EXPECT_NEAR(ks.getVariablePosition("r_shoulder_pan_joint"), .4, std::numeric_limits<double>::epsilon());
00274 EXPECT_TRUE(jc1.decide(ks).satisfied);
00275 EXPECT_TRUE(jc2.decide(ks).satisfied);
00276 }
00277
00278
00279 jcm1.position = .38;
00280 jcm2.position = .42;
00281 EXPECT_TRUE(jc1.configure(jcm1));
00282 EXPECT_TRUE(jc2.configure(jcm2));
00283 js.push_back(jc1);
00284 js.push_back(jc2);
00285 EXPECT_TRUE(jcs.configure(js));
00286 for (int t = 0 ; t < 100 ; ++t)
00287 {
00288 EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00289 EXPECT_TRUE(jc1.decide(ks).satisfied);
00290 EXPECT_TRUE(jc2.decide(ks).satisfied);
00291 }
00292 }
00293
00294 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple)
00295 {
00296 robot_state::RobotState ks(kmodel);
00297 ks.setToDefaultValues();
00298 robot_state::Transforms &tf = ps->getTransformsNonConst();
00299
00300 kinematic_constraints::PositionConstraint pc(kmodel);
00301 moveit_msgs::PositionConstraint pcm;
00302
00303 pcm.link_name = "l_wrist_roll_link";
00304 pcm.target_point_offset.x = 0;
00305 pcm.target_point_offset.y = 0;
00306 pcm.target_point_offset.z = 0;
00307 pcm.constraint_region.primitives.resize(1);
00308 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00309 pcm.constraint_region.primitives[0].dimensions.resize(1);
00310 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00311
00312 pcm.constraint_region.primitive_poses.resize(1);
00313 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00314 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00315 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00316 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00317 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00318 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00319 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00320 pcm.weight = 1.0;
00321
00322 EXPECT_FALSE(pc.configure(pcm, tf));
00323
00324 constraint_samplers::IKConstraintSampler ik_bad(ps, "l_arm");
00325 EXPECT_FALSE(ik_bad.isValid());
00326
00327 constraint_samplers::IKConstraintSampler iks(ps, "left_arm");
00328 EXPECT_TRUE(iks.isValid());
00329
00330 EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose()));
00331
00332 EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
00333
00334 pcm.header.frame_id = kmodel->getModelFrame();
00335 EXPECT_TRUE(pc.configure(pcm, tf));
00336 EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
00337
00338
00339 constraint_samplers::IKConstraintSampler ik_bad_2(ps, "right_arm");
00340 EXPECT_TRUE(ik_bad_2.isValid());
00341 EXPECT_FALSE(ik_bad_2.configure(constraint_samplers::IKSamplingPose(pc)));
00342
00343
00344 pcm.link_name = "l_shoulder_pan_link";
00345 EXPECT_TRUE(pc.configure(pcm, tf));
00346 EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
00347
00348
00349 constraint_samplers::IKConstraintSampler ik_base(ps, "base");
00350 EXPECT_TRUE(ik_base.isValid());
00351 pcm.link_name = "l_wrist_roll_link";
00352 EXPECT_TRUE(pc.configure(pcm, tf));
00353 EXPECT_FALSE(ik_base.configure(constraint_samplers::IKSamplingPose(pc)));
00354
00355
00356 constraint_samplers::IKConstraintSampler ik_arms(ps, "arms");
00357 EXPECT_FALSE(iks.isValid());
00358 }
00359
00360
00361 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler)
00362 {
00363 robot_state::RobotState ks(kmodel);
00364 ks.setToDefaultValues();
00365 robot_state::RobotState ks_const(kmodel);
00366 ks_const.setToDefaultValues();
00367
00368 robot_state::Transforms &tf = ps->getTransformsNonConst();
00369
00370 kinematic_constraints::OrientationConstraint oc(kmodel);
00371 moveit_msgs::OrientationConstraint ocm;
00372
00373 ocm.link_name = "r_wrist_roll_link";
00374 ocm.header.frame_id = ocm.link_name;
00375 ocm.orientation.x = 0.5;
00376 ocm.orientation.y = 0.5;
00377 ocm.orientation.z = 0.5;
00378 ocm.orientation.w = 0.5;
00379 ocm.absolute_x_axis_tolerance = 0.01;
00380 ocm.absolute_y_axis_tolerance = 0.01;
00381 ocm.absolute_z_axis_tolerance = 0.01;
00382 ocm.weight = 1.0;
00383
00384 EXPECT_TRUE(oc.configure(ocm, tf));
00385
00386 bool p1 = oc.decide(ks).satisfied;
00387 EXPECT_FALSE(p1);
00388
00389 ocm.header.frame_id = kmodel->getModelFrame();
00390 EXPECT_TRUE(oc.configure(ocm, tf));
00391
00392 constraint_samplers::IKConstraintSampler iks(ps, "right_arm");
00393 EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(oc)));
00394 for (int t = 0 ; t < 100; ++t)
00395 {
00396 EXPECT_TRUE(iks.sample(ks, ks_const, 100));
00397 EXPECT_TRUE(oc.decide(ks).satisfied);
00398 }
00399 }
00400
00401 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid)
00402 {
00403 robot_state::RobotState ks(kmodel);
00404 ks.setToDefaultValues();
00405 robot_state::RobotState ks_const(kmodel);
00406 ks_const.setToDefaultValues();
00407
00408 robot_state::Transforms &tf = ps->getTransformsNonConst();
00409
00410 kinematic_constraints::PositionConstraint pc(kmodel);
00411 moveit_msgs::PositionConstraint pcm;
00412
00413 pcm.link_name = "l_wrist_roll_link";
00414 pcm.target_point_offset.x = 0;
00415 pcm.target_point_offset.y = 0;
00416 pcm.target_point_offset.z = 0;
00417 pcm.constraint_region.primitives.resize(1);
00418 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00419 pcm.constraint_region.primitives[0].dimensions.resize(1);
00420 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00421
00422 pcm.header.frame_id = kmodel->getModelFrame();
00423
00424 pcm.constraint_region.primitive_poses.resize(1);
00425 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00426 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00427 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00428 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00429 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00430 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00431 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00432 pcm.weight = 1.0;
00433
00434 EXPECT_TRUE(pc.configure(pcm, tf));
00435
00436 kinematic_constraints::OrientationConstraint oc(kmodel);
00437 moveit_msgs::OrientationConstraint ocm;
00438
00439 ocm.link_name = "l_wrist_roll_link";
00440 ocm.header.frame_id = kmodel->getModelFrame();
00441 ocm.orientation.x = 0.0;
00442 ocm.orientation.y = 0.0;
00443 ocm.orientation.z = 0.0;
00444 ocm.orientation.w = 1.0;
00445 ocm.absolute_x_axis_tolerance = 0.2;
00446 ocm.absolute_y_axis_tolerance = 0.1;
00447 ocm.absolute_z_axis_tolerance = 0.4;
00448 ocm.weight = 1.0;
00449
00450 EXPECT_TRUE(oc.configure(ocm, tf));
00451
00452 constraint_samplers::IKConstraintSampler iks1(ps, "left_arm");
00453 EXPECT_TRUE(iks1.configure(constraint_samplers::IKSamplingPose(pc, oc)));
00454 for (int t = 0 ; t < 100 ; ++t)
00455 {
00456 EXPECT_TRUE(iks1.sample(ks, ks_const, 100));
00457 EXPECT_TRUE(pc.decide(ks).satisfied);
00458 EXPECT_TRUE(oc.decide(ks).satisfied);
00459 }
00460
00461 constraint_samplers::IKConstraintSampler iks2(ps, "left_arm");
00462 EXPECT_TRUE(iks2.configure(constraint_samplers::IKSamplingPose(pc)));
00463 for (int t = 0 ; t < 100 ; ++t)
00464 {
00465 EXPECT_TRUE(iks2.sample(ks, ks_const, 100));
00466 EXPECT_TRUE(pc.decide(ks).satisfied);
00467 }
00468
00469 constraint_samplers::IKConstraintSampler iks3(ps, "left_arm");
00470 EXPECT_TRUE(iks3.configure(constraint_samplers::IKSamplingPose(oc)));
00471 for (int t = 0 ; t < 100 ; ++t)
00472 {
00473 EXPECT_TRUE(iks3.sample(ks, ks_const, 100));
00474 EXPECT_TRUE(oc.decide(ks).satisfied);
00475 }
00476 }
00477
00478 TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
00479 {
00480 robot_state::RobotState ks(kmodel);
00481 ks.setToDefaultValues();
00482
00483 robot_state::RobotState ks_const(kmodel);
00484 ks_const.setToDefaultValues();
00485
00486 robot_state::Transforms &tf = ps->getTransformsNonConst();
00487
00488 kinematic_constraints::JointConstraint jc1(kmodel);
00489
00490 std::map<std::string, double> state_values;
00491
00492
00493 moveit_msgs::JointConstraint torso_constraint;
00494 torso_constraint.joint_name = "torso_lift_joint";
00495 torso_constraint.position = state_values["torso_lift_joint"];
00496 torso_constraint.tolerance_above = 0.01;
00497 torso_constraint.tolerance_below = 0.01;
00498 torso_constraint.weight = 1.0;
00499 EXPECT_TRUE(jc1.configure(torso_constraint));
00500
00501 kinematic_constraints::JointConstraint jc2(kmodel);
00502 moveit_msgs::JointConstraint jcm2;
00503 jcm2.joint_name = "r_elbow_flex_joint";
00504 jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
00505 jcm2.tolerance_above = 0.01;
00506 jcm2.tolerance_below = 0.01;
00507 jcm2.weight = 1.0;
00508 EXPECT_TRUE(jc2.configure(jcm2));
00509
00510 moveit_msgs::PositionConstraint pcm;
00511
00512 pcm.link_name = "l_wrist_roll_link";
00513 pcm.target_point_offset.x = 0;
00514 pcm.target_point_offset.y = 0;
00515 pcm.target_point_offset.z = 0;
00516 pcm.constraint_region.primitives.resize(1);
00517 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00518 pcm.constraint_region.primitives[0].dimensions.resize(1);
00519 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00520
00521 pcm.constraint_region.primitive_poses.resize(1);
00522 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00523 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00524 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00525 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00526 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00527 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00528 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00529 pcm.weight = 1.0;
00530
00531 pcm.header.frame_id = kmodel->getModelFrame();
00532
00533 moveit_msgs::OrientationConstraint ocm;
00534
00535 ocm.link_name = "l_wrist_roll_link";
00536 ocm.header.frame_id = kmodel->getModelFrame();
00537 ocm.orientation.x = 0.0;
00538 ocm.orientation.y = 0.0;
00539 ocm.orientation.z = 0.0;
00540 ocm.orientation.w = 1.0;
00541 ocm.absolute_x_axis_tolerance = 0.2;
00542 ocm.absolute_y_axis_tolerance = 0.1;
00543 ocm.absolute_z_axis_tolerance = 0.4;
00544 ocm.weight = 1.0;
00545
00546 std::vector<kinematic_constraints::JointConstraint> js;
00547 js.push_back(jc1);
00548
00549 boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp(new constraint_samplers::JointConstraintSampler(ps, "arms_and_torso"));
00550 EXPECT_TRUE(jcsp->configure(js));
00551
00552 std::vector<kinematic_constraints::JointConstraint> js2;
00553 js2.push_back(jc2);
00554
00555 boost::shared_ptr<constraint_samplers::JointConstraintSampler> jcsp2(new constraint_samplers::JointConstraintSampler(ps, "arms"));
00556 EXPECT_TRUE(jcsp2->configure(js2));
00557
00558 kinematic_constraints::PositionConstraint pc(kmodel);
00559 EXPECT_TRUE(pc.configure(pcm, tf));
00560
00561 kinematic_constraints::OrientationConstraint oc(kmodel);
00562 EXPECT_TRUE(oc.configure(ocm, tf));
00563
00564 boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp(new constraint_samplers::IKConstraintSampler(ps, "left_arm"));
00565 EXPECT_TRUE(iksp->isValid());
00566 EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
00567
00568 std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
00569 cspv.push_back(jcsp2);
00570 cspv.push_back(iksp);
00571 cspv.push_back(jcsp);
00572
00573 constraint_samplers::UnionConstraintSampler ucs(ps, "arms_and_torso", cspv);
00574
00575
00576 constraint_samplers::JointConstraintSampler* jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
00577 EXPECT_TRUE(jcs);
00578 EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
00579
00580 constraint_samplers::JointConstraintSampler* jcs2 = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
00581 EXPECT_TRUE(jcs2);
00582 EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
00583
00584 for (int t = 0 ; t < 100; ++t)
00585 {
00586 EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
00587 EXPECT_TRUE(jc1.decide(ks).satisfied);
00588 EXPECT_TRUE(jc2.decide(ks).satisfied);
00589 EXPECT_TRUE(pc.decide(ks).satisfied);
00590 }
00591
00592
00593 pcm.link_name = "r_wrist_roll_link";
00594 ocm.link_name = "r_wrist_roll_link";
00595 cspv.clear();
00596
00597 kinematic_constraints::PositionConstraint pc2(kmodel);
00598 EXPECT_TRUE(pc2.configure(pcm, tf));
00599
00600 kinematic_constraints::OrientationConstraint oc2(kmodel);
00601 EXPECT_TRUE(oc2.configure(ocm, tf));
00602
00603 boost::shared_ptr<constraint_samplers::IKConstraintSampler> iksp2(new constraint_samplers::IKConstraintSampler(ps, "right_arm"));
00604 EXPECT_TRUE(iksp2->isValid());
00605 EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
00606
00607
00608 cspv.clear();
00609 cspv.push_back(iksp2);
00610 cspv.push_back(iksp);
00611
00612 constraint_samplers::UnionConstraintSampler ucs2(ps, "arms_and_torso", cspv);
00613
00614 constraint_samplers::IKConstraintSampler* ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
00615 ASSERT_TRUE(ikcs_test);
00616 EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
00617
00618
00619 pcm.link_name = "l_wrist_roll_link";
00620 ocm.link_name = "l_wrist_roll_link";
00621 pcm.header.frame_id = "r_wrist_roll_link";
00622 ocm.header.frame_id = "r_wrist_roll_link";
00623 EXPECT_TRUE(pc.configure(pcm, tf));
00624 EXPECT_TRUE(oc.configure(ocm, tf));
00625 ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
00626
00627 cspv.clear();
00628 cspv.push_back(iksp2);
00629 cspv.push_back(iksp);
00630
00631 constraint_samplers::UnionConstraintSampler ucs3(ps, "arms_and_torso", cspv);
00632
00633 ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
00634 EXPECT_TRUE(ikcs_test);
00635 EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
00636 }
00637
00638 TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager)
00639 {
00640 robot_state::RobotState ks(kmodel);
00641 ks.setToDefaultValues();
00642 robot_state::RobotState ks_const(kmodel);
00643 ks_const.setToDefaultValues();
00644
00645 robot_state::Transforms &tf = ps->getTransformsNonConst();
00646
00647 kinematic_constraints::PositionConstraint pc(kmodel);
00648
00649 moveit_msgs::PositionConstraint pcm;
00650 pcm.link_name = "l_wrist_roll_link";
00651 pcm.target_point_offset.x = 0;
00652 pcm.target_point_offset.y = 0;
00653 pcm.target_point_offset.z = 0;
00654 pcm.constraint_region.primitives.resize(1);
00655 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00656 pcm.constraint_region.primitives[0].dimensions.resize(1);
00657 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00658
00659 pcm.header.frame_id = kmodel->getModelFrame();
00660
00661 pcm.constraint_region.primitive_poses.resize(1);
00662 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00663 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00664 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00665 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00666 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00667 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00668 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00669 pcm.weight = 1.0;
00670
00671 moveit_msgs::OrientationConstraint ocm;
00672
00673 ocm.link_name = "l_wrist_roll_link";
00674 ocm.header.frame_id = kmodel->getModelFrame();
00675 ocm.orientation.x = 0.0;
00676 ocm.orientation.y = 0.0;
00677 ocm.orientation.z = 0.0;
00678 ocm.orientation.w = 1.0;
00679 ocm.absolute_x_axis_tolerance = 0.2;
00680 ocm.absolute_y_axis_tolerance = 0.1;
00681 ocm.absolute_z_axis_tolerance = 0.4;
00682 ocm.weight = 1.0;
00683
00684
00685 moveit_msgs::Constraints c;
00686 c.position_constraints.push_back(pcm);
00687 c.orientation_constraints.push_back(ocm);
00688
00689 constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", c);
00690 EXPECT_TRUE(s.get() != NULL);
00691 constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
00692 ASSERT_TRUE(iks);
00693 ASSERT_TRUE(iks->getPositionConstraint());
00694 ASSERT_TRUE(iks->getOrientationConstraint());
00695
00696 static const int NT = 100;
00697 int succ = 0;
00698 for (int t = 0 ; t < NT ; ++t)
00699 {
00700 EXPECT_TRUE(s->sample(ks, ks_const, 100));
00701 EXPECT_TRUE(iks->getPositionConstraint()->decide(ks).satisfied);
00702 EXPECT_TRUE(iks->getOrientationConstraint()->decide(ks).satisfied);
00703 if (s->sample(ks, ks_const, 1))
00704 succ++;
00705 }
00706 ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf", (double)succ / (double)NT);
00707
00708
00709 ocm.absolute_x_axis_tolerance = 0.1;
00710
00711 c.orientation_constraints.push_back(ocm);
00712
00713 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", c);
00714 EXPECT_TRUE(s.get() != NULL);
00715
00716 iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
00717 ASSERT_TRUE(iks);
00718 ASSERT_TRUE(iks->getOrientationConstraint());
00719 EXPECT_NEAR(iks->getOrientationConstraint()->getXAxisTolerance(),.1, .0001);
00720 }
00721
00722 TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager)
00723 {
00724 robot_state::RobotState ks(kmodel);
00725 ks.setToDefaultValues();
00726
00727 moveit_msgs::Constraints con;
00728 con.joint_constraints.resize(1);
00729
00730 con.joint_constraints[0].joint_name = "l_shoulder_pan_joint";
00731 con.joint_constraints[0].position = 0.54;
00732 con.joint_constraints[0].tolerance_above = 0.01;
00733 con.joint_constraints[0].tolerance_below = 0.01;
00734 con.joint_constraints[0].weight = 1.0;
00735
00736 constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "right_arm", con);
00737 EXPECT_FALSE(s);
00738 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00739 EXPECT_TRUE(s);
00740
00741 con.joint_constraints.resize(7);
00742
00743 con.joint_constraints[1].joint_name = "l_shoulder_lift_joint";
00744 con.joint_constraints[1].position = 0.54;
00745 con.joint_constraints[1].tolerance_above = 0.01;
00746 con.joint_constraints[1].tolerance_below = 0.01;
00747 con.joint_constraints[1].weight = 1.0;
00748
00749 con.joint_constraints[2].joint_name = "l_upper_arm_roll_joint";
00750 con.joint_constraints[2].position = 0.54;
00751 con.joint_constraints[2].tolerance_above = 0.01;
00752 con.joint_constraints[2].tolerance_below = 0.01;
00753 con.joint_constraints[2].weight = 1.0;
00754
00755 con.joint_constraints[3].joint_name = "l_elbow_flex_joint";
00756 con.joint_constraints[3].position = -0.54;
00757 con.joint_constraints[3].tolerance_above = 0.01;
00758 con.joint_constraints[3].tolerance_below = 0.01;
00759 con.joint_constraints[3].weight = 1.0;
00760
00761 con.joint_constraints[4].joint_name = "l_forearm_roll_joint";
00762 con.joint_constraints[4].position = 0.54;
00763 con.joint_constraints[4].tolerance_above = 0.01;
00764 con.joint_constraints[4].tolerance_below = 0.01;
00765 con.joint_constraints[4].weight = 1.0;
00766
00767 con.joint_constraints[5].joint_name = "l_wrist_flex_joint";
00768 con.joint_constraints[5].position = -0.54;
00769 con.joint_constraints[5].tolerance_above = 0.05;
00770 con.joint_constraints[5].tolerance_below = 0.05;
00771 con.joint_constraints[5].weight = 1.0;
00772
00773
00774 con.joint_constraints[6].joint_name = "l_wrist_flex_joint";
00775 con.joint_constraints[6].position = -0.56;
00776 con.joint_constraints[6].tolerance_above = 0.01;
00777 con.joint_constraints[6].tolerance_below = 0.01;
00778 con.joint_constraints[6].weight = 1.0;
00779
00780 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00781 EXPECT_TRUE(s);
00782
00783 con.position_constraints.resize(1);
00784
00785
00786 con.position_constraints[0].link_name = "r_wrist_roll_link";
00787 con.position_constraints[0].target_point_offset.x = 0;
00788 con.position_constraints[0].target_point_offset.y = 0;
00789 con.position_constraints[0].target_point_offset.z = 0;
00790 con.position_constraints[0].constraint_region.primitives.resize(1);
00791 con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00792 con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
00793 con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
00794
00795 con.position_constraints[0].header.frame_id = kmodel->getModelFrame();
00796
00797 con.position_constraints[0].constraint_region.primitive_poses.resize(1);
00798 con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
00799 con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
00800 con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
00801 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
00802 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
00803 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
00804 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
00805 con.position_constraints[0].weight = 1.0;
00806
00807
00808 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00809 EXPECT_TRUE(s);
00810 constraint_samplers::JointConstraintSampler* jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
00811 EXPECT_TRUE(jcs);
00812
00813 con.position_constraints[0].link_name = "l_wrist_roll_link";
00814 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00815 EXPECT_TRUE(s);
00816 jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
00817 EXPECT_FALSE(jcs);
00818 constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
00819 EXPECT_FALSE(iks);
00820
00821
00822 constraint_samplers::UnionConstraintSampler* ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00823 EXPECT_TRUE(ucs);
00824
00825 con.orientation_constraints.resize(1);
00826
00827
00828 con.orientation_constraints[0].link_name = "r_wrist_roll_link";
00829 con.orientation_constraints[0].header.frame_id = kmodel->getModelFrame();
00830 con.orientation_constraints[0].orientation.x = 0.0;
00831 con.orientation_constraints[0].orientation.y = 0.0;
00832 con.orientation_constraints[0].orientation.z = 0.0;
00833 con.orientation_constraints[0].orientation.w = 1.0;
00834 con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
00835 con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
00836 con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
00837 con.orientation_constraints[0].weight = 1.0;
00838
00839
00840 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00841 EXPECT_TRUE(s);
00842 ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00843 ASSERT_TRUE(ucs);
00844 jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs->getSamplers()[0].get());
00845 iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00846
00847 ASSERT_TRUE(iks);
00848 ASSERT_TRUE(jcs);
00849 EXPECT_TRUE(iks->getPositionConstraint());
00850 EXPECT_FALSE(iks->getOrientationConstraint());
00851
00852 con.orientation_constraints[0].link_name = "l_wrist_roll_link";
00853
00854
00855 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00856 EXPECT_TRUE(s);
00857 ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00858 iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00859 ASSERT_TRUE(iks);
00860 EXPECT_TRUE(iks->getPositionConstraint());
00861 EXPECT_TRUE(iks->getOrientationConstraint());
00862
00863
00864 con.position_constraints[0].link_name = "r_wrist_roll_link";
00865 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00866 ASSERT_TRUE(s);
00867 ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00868 iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00869 ASSERT_TRUE(iks);
00870 EXPECT_FALSE(iks->getPositionConstraint());
00871 EXPECT_TRUE(iks->getOrientationConstraint());
00872
00873
00874 con.joint_constraints.resize(8);
00875 con.joint_constraints[7].joint_name = "l_wrist_roll_joint";
00876 con.joint_constraints[7].position = 0.54;
00877 con.joint_constraints[7].tolerance_above = 0.01;
00878 con.joint_constraints[7].tolerance_below = 0.01;
00879 con.joint_constraints[7].weight = 1.0;
00880
00881 s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00882 EXPECT_TRUE(s);
00883 jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
00884 ASSERT_TRUE(jcs);
00885 }
00886
00887 TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager)
00888 {
00889 robot_state::RobotState ks(kmodel);
00890 ks.setToDefaultValues();
00891 robot_state::RobotState ks_const(kmodel);
00892 ks_const.setToDefaultValues();
00893
00894 robot_state::Transforms &tf = ps->getTransformsNonConst();
00895
00896 std::map<std::string, double> state_values;
00897
00898
00899 moveit_msgs::Constraints con;
00900 con.joint_constraints.resize(1);
00901
00902 con.joint_constraints[0].joint_name = "torso_lift_joint";
00903 con.joint_constraints[0].position = ks.getVariablePosition("torso_lift_joint");
00904 con.joint_constraints[0].tolerance_above = 0.01;
00905 con.joint_constraints[0].tolerance_below = 0.01;
00906 con.joint_constraints[0].weight = 1.0;
00907
00908 kinematic_constraints::JointConstraint jc(kmodel);
00909 EXPECT_TRUE(jc.configure(con.joint_constraints[0]));
00910
00911 con.position_constraints.resize(1);
00912
00913 con.position_constraints[0].link_name = "l_wrist_roll_link";
00914 con.position_constraints[0].target_point_offset.x = 0;
00915 con.position_constraints[0].target_point_offset.y = 0;
00916 con.position_constraints[0].target_point_offset.z = 0;
00917 con.position_constraints[0].constraint_region.primitives.resize(1);
00918 con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00919 con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
00920 con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
00921
00922 con.position_constraints[0].constraint_region.primitive_poses.resize(1);
00923 con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
00924 con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
00925 con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
00926 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
00927 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
00928 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
00929 con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
00930 con.position_constraints[0].weight = 1.0;
00931
00932 con.position_constraints[0].header.frame_id = kmodel->getModelFrame();
00933
00934 con.orientation_constraints.resize(1);
00935 con.orientation_constraints[0].link_name = "l_wrist_roll_link";
00936 con.orientation_constraints[0].header.frame_id = kmodel->getModelFrame();
00937 con.orientation_constraints[0].orientation.x = 0.0;
00938 con.orientation_constraints[0].orientation.y = 0.0;
00939 con.orientation_constraints[0].orientation.z = 0.0;
00940 con.orientation_constraints[0].orientation.w = 1.0;
00941 con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
00942 con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
00943 con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
00944 con.orientation_constraints[0].weight = 1.0;
00945
00946 constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms_and_torso", con);
00947
00948 constraint_samplers::UnionConstraintSampler* ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00949 ASSERT_TRUE(ucs);
00950
00951 constraint_samplers::IKConstraintSampler* ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00952
00953 ASSERT_TRUE(ikcs_test);
00954
00955 for (int t = 0 ; t < 1; ++t)
00956 {
00957 EXPECT_TRUE(s->sample(ks, ks_const, 1));
00958 EXPECT_TRUE(jc.decide(ks).satisfied);
00959 EXPECT_TRUE(ikcs_test->getPositionConstraint()->decide(ks).satisfied);
00960 EXPECT_TRUE(ikcs_test->getOrientationConstraint()->decide(ks).satisfied);
00961 }
00962 }
00963
00964 TEST_F(LoadPlanningModelsPr2, SubgroupJointConstraintsSamplerManager)
00965 {
00966 robot_state::RobotState ks(kmodel);
00967 ks.setToDefaultValues();
00968 robot_state::RobotState ks_const(kmodel);
00969 ks_const.setToDefaultValues();
00970
00971 robot_state::Transforms &tf = ps->getTransformsNonConst();
00972
00973 kinematic_constraints::JointConstraint jc1(kmodel);
00974 moveit_msgs::JointConstraint jcm1;
00975 jcm1.joint_name = "head_pan_joint";
00976 jcm1.position = 0.42;
00977 jcm1.tolerance_above = 0.01;
00978 jcm1.tolerance_below = 0.05;
00979 jcm1.weight = 1.0;
00980 EXPECT_TRUE(jc1.configure(jcm1));
00981
00982 kinematic_constraints::JointConstraint jc2(kmodel);
00983 moveit_msgs::JointConstraint jcm2;
00984 jcm2.joint_name = "l_shoulder_pan_joint";
00985 jcm2.position = 0.9;
00986 jcm2.tolerance_above = 0.1;
00987 jcm2.tolerance_below = 0.05;
00988 jcm2.weight = 1.0;
00989 EXPECT_TRUE(jc2.configure(jcm2));
00990
00991 kinematic_constraints::JointConstraint jc3(kmodel);
00992 moveit_msgs::JointConstraint jcm3;
00993 jcm3.joint_name = "r_wrist_roll_joint";
00994 jcm3.position = 0.7;
00995 jcm3.tolerance_above = 0.14;
00996 jcm3.tolerance_below = 0.005;
00997 jcm3.weight = 1.0;
00998 EXPECT_TRUE(jc3.configure(jcm3));
00999
01000 kinematic_constraints::JointConstraint jc4(kmodel);
01001 moveit_msgs::JointConstraint jcm4;
01002 jcm4.joint_name = "torso_lift_joint";
01003 jcm4.position = 0.2;
01004 jcm4.tolerance_above = 0.09;
01005 jcm4.tolerance_below = 0.01;
01006 jcm4.weight = 1.0;
01007 EXPECT_TRUE(jc4.configure(jcm4));
01008
01009 std::vector<kinematic_constraints::JointConstraint> js;
01010 js.push_back(jc1);
01011 js.push_back(jc2);
01012 js.push_back(jc3);
01013 js.push_back(jc4);
01014
01015 constraint_samplers::JointConstraintSampler jcs(ps, "arms");
01016 jcs.configure(js);
01017 EXPECT_EQ(jcs.getConstrainedJointCount(), 2);
01018 EXPECT_EQ(jcs.getUnconstrainedJointCount(), 12);
01019
01020 for (int t = 0 ; t < 100 ; ++t)
01021 {
01022 EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
01023 EXPECT_TRUE(jc2.decide(ks).satisfied);
01024 EXPECT_TRUE(jc3.decide(ks).satisfied);
01025 }
01026
01027
01028 moveit_msgs::Constraints c;
01029
01030
01031 constraint_samplers::ConstraintSamplerPtr s0 = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01032 EXPECT_TRUE(s0.get() == NULL);
01033
01034
01035 c.joint_constraints.push_back(jcm1);
01036 c.joint_constraints.push_back(jcm2);
01037 c.joint_constraints.push_back(jcm3);
01038 c.joint_constraints.push_back(jcm4);
01039
01040 constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01041 EXPECT_TRUE(s.get() != NULL);
01042
01043
01044 for (int t = 0 ; t < 1000 ; ++t)
01045 {
01046 EXPECT_TRUE(s->sample(ks, ks_const, 1));
01047 EXPECT_TRUE(jc2.decide(ks).satisfied);
01048 EXPECT_TRUE(jc3.decide(ks).satisfied);
01049 }
01050 }
01051
01052 TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
01053 {
01054 moveit_msgs::Constraints c;
01055
01056 moveit_msgs::PositionConstraint pcm;
01057 pcm.link_name = "l_wrist_roll_link";
01058 pcm.target_point_offset.x = 0;
01059 pcm.target_point_offset.y = 0;
01060 pcm.target_point_offset.z = 0;
01061
01062 pcm.constraint_region.primitives.resize(1);
01063 pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
01064 pcm.constraint_region.primitives[0].dimensions.resize(1);
01065 pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
01066
01067 pcm.header.frame_id = kmodel->getModelFrame();
01068
01069 pcm.constraint_region.primitive_poses.resize(1);
01070 pcm.constraint_region.primitive_poses[0].position.x = 0.55;
01071 pcm.constraint_region.primitive_poses[0].position.y = 0.2;
01072 pcm.constraint_region.primitive_poses[0].position.z = 1.25;
01073 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
01074 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
01075 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
01076 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
01077 pcm.weight = 1.0;
01078 c.position_constraints.push_back(pcm);
01079
01080 moveit_msgs::OrientationConstraint ocm;
01081 ocm.link_name = "l_wrist_roll_link";
01082 ocm.header.frame_id = kmodel->getModelFrame();
01083 ocm.orientation.x = 0.0;
01084 ocm.orientation.y = 0.0;
01085 ocm.orientation.z = 0.0;
01086 ocm.orientation.w = 1.0;
01087 ocm.absolute_x_axis_tolerance = 0.2;
01088 ocm.absolute_y_axis_tolerance = 0.1;
01089 ocm.absolute_z_axis_tolerance = 0.4;
01090 ocm.weight = 1.0;
01091 c.orientation_constraints.push_back(ocm);
01092
01093 ocm.link_name = "r_wrist_roll_link";
01094 ocm.header.frame_id = kmodel->getModelFrame();
01095 ocm.orientation.x = 0.0;
01096 ocm.orientation.y = 0.0;
01097 ocm.orientation.z = 0.0;
01098 ocm.orientation.w = 1.0;
01099 ocm.absolute_x_axis_tolerance = 0.01;
01100 ocm.absolute_y_axis_tolerance = 0.01;
01101 ocm.absolute_z_axis_tolerance = 0.01;
01102 ocm.weight = 1.0;
01103 c.orientation_constraints.push_back(ocm);
01104
01105 robot_state::Transforms &tf = ps->getTransformsNonConst();
01106 constraint_samplers::ConstraintSamplerPtr s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01107 EXPECT_TRUE(s);
01108 constraint_samplers::UnionConstraintSampler* ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
01109 EXPECT_TRUE(ucs);
01110
01111 kinematic_constraints::KinematicConstraintSet kset(kmodel);
01112 kset.add(c, tf);
01113
01114 robot_state::RobotState ks(kmodel);
01115 ks.setToDefaultValues();
01116 robot_state::RobotState ks_const(kmodel);
01117 ks_const.setToDefaultValues();
01118
01119 static const int NT = 100;
01120 int succ = 0;
01121 for (int t = 0 ; t < NT ; ++t)
01122 {
01123 EXPECT_TRUE(s->sample(ks, ks_const, 1000));
01124 EXPECT_TRUE(kset.decide(ks).satisfied);
01125 if (s->sample(ks, ks_const, 1))
01126 succ++;
01127 }
01128 logInform("Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf", (double)succ / (double)NT);
01129 }
01130
01131 int main(int argc, char **argv)
01132 {
01133 testing::InitGoogleTest(&argc, argv);
01134 ros::Time::init();
01135 return RUN_ALL_TESTS();
01136 }