test_constraint_samplers.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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 #include <moveit_resources/config.h>
00046 
00047 #include <geometric_shapes/shape_operations.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   kinematics::KinematicsBasePtr getKinematicsSolverRightArm(const robot_model::JointModelGroup* jmg)
00062   {
00063     {
00064       return pr2_kinematics_plugin_right_arm_;
00065     }
00066   }
00067 
00068   kinematics::KinematicsBasePtr getKinematicsSolverLeftArm(const robot_model::JointModelGroup* jmg)
00069   {
00070     {
00071       return pr2_kinematics_plugin_left_arm_;
00072     }
00073   }
00074 
00075   virtual void SetUp()
00076   {
00077     boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
00078 
00079     srdf_model.reset(new srdf::Model());
00080     std::string xml_string;
00081     std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
00082     if (xml_file.is_open())
00083     {
00084       while (xml_file.good())
00085       {
00086         std::string line;
00087         std::getline(xml_file, line);
00088         xml_string += (line + "\n");
00089       }
00090       xml_file.close();
00091       urdf_model = urdf::parseURDF(xml_string);
00092     }
00093     srdf_model->initFile(*urdf_model, (res_path / "pr2_description/srdf/robot.xml").string());
00094     kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
00095 
00096     pr2_kinematics_plugin_right_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
00097 
00098     pr2_kinematics_plugin_right_arm_->setRobotModel(urdf_model);
00099     pr2_kinematics_plugin_right_arm_->initialize("", "right_arm", "torso_lift_link", "r_wrist_roll_link", .01);
00100 
00101     pr2_kinematics_plugin_left_arm_.reset(new pr2_arm_kinematics::PR2ArmKinematicsPlugin);
00102 
00103     pr2_kinematics_plugin_left_arm_->setRobotModel(urdf_model);
00104     pr2_kinematics_plugin_left_arm_->initialize("", "left_arm", "torso_lift_link", "l_wrist_roll_link", .01);
00105 
00106     func_right_arm = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1);
00107     func_left_arm = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1);
00108 
00109     std::map<std::string, robot_model::SolverAllocatorFn> allocators;
00110     allocators["right_arm"] = func_right_arm;
00111     allocators["left_arm"] = func_left_arm;
00112     allocators["whole_body"] = func_right_arm;
00113     allocators["base"] = func_left_arm;
00114 
00115     kmodel->setKinematicsAllocators(allocators);
00116 
00117     ps.reset(new planning_scene::PlanningScene(kmodel));
00118   };
00119 
00120   virtual void TearDown()
00121   {
00122   }
00123 
00124 protected:
00125   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00126   boost::shared_ptr<srdf::Model> srdf_model;
00127   robot_model::RobotModelPtr kmodel;
00128   planning_scene::PlanningScenePtr ps;
00129   pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_right_arm_;
00130   pr2_arm_kinematics::PR2ArmKinematicsPluginPtr pr2_kinematics_plugin_left_arm_;
00131   robot_model::SolverAllocatorFn func_right_arm;
00132   robot_model::SolverAllocatorFn func_left_arm;
00133 };
00134 
00135 TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSimple)
00136 {
00137   robot_state::RobotState ks(kmodel);
00138   ks.setToDefaultValues();
00139 
00140   robot_state::RobotState ks_const(kmodel);
00141   ks_const.setToDefaultValues();
00142 
00143   kinematic_constraints::JointConstraint jc1(kmodel);
00144   moveit_msgs::JointConstraint jcm1;
00145   // leaving off joint name
00146   jcm1.position = 0.42;
00147   jcm1.tolerance_above = 0.01;
00148   jcm1.tolerance_below = 0.05;
00149   jcm1.weight = 1.0;
00150   EXPECT_FALSE(jc1.configure(jcm1));
00151 
00152   std::vector<kinematic_constraints::JointConstraint> js;
00153   js.push_back(jc1);
00154 
00155   constraint_samplers::JointConstraintSampler jcs(ps, "right_arm");
00156   // no valid constraints
00157   EXPECT_FALSE(jcs.configure(js));
00158 
00159   // testing that this does the right thing
00160   jcm1.joint_name = "r_shoulder_pan_joint";
00161   EXPECT_TRUE(jc1.configure(jcm1));
00162   js.push_back(jc1);
00163   EXPECT_TRUE(jcs.configure(js));
00164   EXPECT_EQ(jcs.getConstrainedJointCount(), 1);
00165   EXPECT_EQ(jcs.getUnconstrainedJointCount(), 6);
00166   EXPECT_TRUE(jcs.sample(ks, ks, 1));
00167 
00168   for (int t = 0; t < 100; ++t)
00169   {
00170     EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00171     EXPECT_TRUE(jc1.decide(ks).satisfied);
00172   }
00173 
00174   // redoing the configure leads to 6 unconstrained variables as well
00175   EXPECT_TRUE(jcs.configure(js));
00176   EXPECT_EQ(jcs.getUnconstrainedJointCount(), 6);
00177 
00178   kinematic_constraints::JointConstraint jc2(kmodel);
00179 
00180   moveit_msgs::JointConstraint jcm2;
00181   jcm2.joint_name = "r_shoulder_pan_joint";
00182   jcm2.position = 0.54;
00183   jcm2.tolerance_above = 0.01;
00184   jcm2.tolerance_below = 0.01;
00185   jcm2.weight = 1.0;
00186   EXPECT_TRUE(jc2.configure(jcm2));
00187   js.push_back(jc2);
00188 
00189   // creating a constraint that conflicts with the other (leaves no sampleable region)
00190   EXPECT_FALSE(jcs.configure(js));
00191   EXPECT_FALSE(jcs.sample(ks, ks_const, 1));
00192 
00193   // we can't sample for a different group
00194   constraint_samplers::JointConstraintSampler jcs2(ps, "arms");
00195   jcs2.configure(js);
00196   EXPECT_FALSE(jcs2.sample(ks, ks_const, 1));
00197 
00198   // not ok to not have any references to joints in this group in the constraints
00199   constraint_samplers::JointConstraintSampler jcs3(ps, "left_arm");
00200   EXPECT_FALSE(jcs3.configure(js));
00201 
00202   // testing that the most restrictive bounds are used
00203   js.clear();
00204 
00205   jcm1.position = .4;
00206   jcm1.tolerance_above = .05;
00207   jcm1.tolerance_below = .05;
00208   jcm2.position = .4;
00209   jcm2.tolerance_above = .1;
00210   jcm2.tolerance_below = .1;
00211   EXPECT_TRUE(jc1.configure(jcm1));
00212   EXPECT_TRUE(jc2.configure(jcm2));
00213   js.push_back(jc1);
00214   js.push_back(jc2);
00215 
00216   EXPECT_TRUE(jcs.configure(js));
00217 
00218   // should always be within narrower constraints
00219   for (int t = 0; t < 100; ++t)
00220   {
00221     EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00222     EXPECT_TRUE(jc1.decide(ks).satisfied);
00223   }
00224 
00225   // too narrow range outside of joint limits
00226   js.clear();
00227 
00228   jcm1.position = -3.1;
00229   jcm1.tolerance_above = .05;
00230   jcm1.tolerance_below = .05;
00231 
00232   // the joint configuration corrects this
00233   EXPECT_TRUE(jc1.configure(jcm1));
00234   js.push_back(jc1);
00235   EXPECT_TRUE(jcs.configure(js));
00236 
00237   // partially overlapping regions will also work
00238   js.clear();
00239   jcm1.position = .35;
00240   jcm1.tolerance_above = .05;
00241   jcm1.tolerance_below = .05;
00242   jcm2.position = .45;
00243   jcm2.tolerance_above = .05;
00244   jcm2.tolerance_below = .05;
00245   EXPECT_TRUE(jc1.configure(jcm1));
00246   EXPECT_TRUE(jc2.configure(jcm2));
00247   js.push_back(jc1);
00248   js.push_back(jc2);
00249 
00250   // leads to a min and max of .4, so all samples should be exactly .4
00251   EXPECT_TRUE(jcs.configure(js));
00252   for (int t = 0; t < 100; ++t)
00253   {
00254     EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
00255     std::map<std::string, double> var_values;
00256     EXPECT_NEAR(ks.getVariablePosition("r_shoulder_pan_joint"), .4, std::numeric_limits<double>::epsilon());
00257     EXPECT_TRUE(jc1.decide(ks).satisfied);
00258     EXPECT_TRUE(jc2.decide(ks).satisfied);
00259   }
00260 
00261   // this leads to a larger sampleable region
00262   jcm1.position = .38;
00263   jcm2.position = .42;
00264   EXPECT_TRUE(jc1.configure(jcm1));
00265   EXPECT_TRUE(jc2.configure(jcm2));
00266   js.push_back(jc1);
00267   js.push_back(jc2);
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     EXPECT_TRUE(jc1.decide(ks).satisfied);
00273     EXPECT_TRUE(jc2.decide(ks).satisfied);
00274   }
00275 }
00276 
00277 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSimple)
00278 {
00279   robot_state::Transforms& tf = ps->getTransformsNonConst();
00280 
00281   kinematic_constraints::PositionConstraint pc(kmodel);
00282   moveit_msgs::PositionConstraint pcm;
00283 
00284   pcm.link_name = "l_wrist_roll_link";
00285   pcm.target_point_offset.x = 0;
00286   pcm.target_point_offset.y = 0;
00287   pcm.target_point_offset.z = 0;
00288   pcm.constraint_region.primitives.resize(1);
00289   pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00290   pcm.constraint_region.primitives[0].dimensions.resize(1);
00291   pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00292 
00293   pcm.constraint_region.primitive_poses.resize(1);
00294   pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00295   pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00296   pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00297   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00298   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00299   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00300   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00301   pcm.weight = 1.0;
00302 
00303   EXPECT_FALSE(pc.configure(pcm, tf));
00304 
00305   constraint_samplers::IKConstraintSampler ik_bad(ps, "l_arm");
00306   EXPECT_FALSE(ik_bad.isValid());
00307 
00308   constraint_samplers::IKConstraintSampler iks(ps, "left_arm");
00309   EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose()));
00310   EXPECT_FALSE(iks.isValid());
00311 
00312   EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
00313 
00314   pcm.header.frame_id = kmodel->getModelFrame();
00315   EXPECT_TRUE(pc.configure(pcm, tf));
00316   EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
00317 
00318   // ik link not in this group
00319   constraint_samplers::IKConstraintSampler ik_bad_2(ps, "right_arm");
00320   EXPECT_FALSE(ik_bad_2.configure(constraint_samplers::IKSamplingPose(pc)));
00321   EXPECT_FALSE(ik_bad_2.isValid());
00322 
00323   // not the ik link
00324   pcm.link_name = "l_shoulder_pan_link";
00325   EXPECT_TRUE(pc.configure(pcm, tf));
00326   EXPECT_FALSE(iks.configure(constraint_samplers::IKSamplingPose(pc)));
00327 
00328   // solver for base doesn't cover group
00329   constraint_samplers::IKConstraintSampler ik_base(ps, "base");
00330   pcm.link_name = "l_wrist_roll_link";
00331   EXPECT_TRUE(pc.configure(pcm, tf));
00332   EXPECT_FALSE(ik_base.configure(constraint_samplers::IKSamplingPose(pc)));
00333   EXPECT_FALSE(ik_base.isValid());
00334 
00335   // shouldn't work as no direct constraint solver
00336   constraint_samplers::IKConstraintSampler ik_arms(ps, "arms");
00337   EXPECT_FALSE(iks.isValid());
00338 }
00339 
00340 TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSampler)
00341 {
00342   robot_state::RobotState ks(kmodel);
00343   ks.setToDefaultValues();
00344   ks.update();
00345   robot_state::RobotState ks_const(kmodel);
00346   ks_const.setToDefaultValues();
00347   ks_const.update();
00348 
00349   robot_state::Transforms& tf = ps->getTransformsNonConst();
00350 
00351   kinematic_constraints::OrientationConstraint oc(kmodel);
00352   moveit_msgs::OrientationConstraint ocm;
00353 
00354   ocm.link_name = "r_wrist_roll_link";
00355   ocm.header.frame_id = ocm.link_name;
00356   ocm.orientation.x = 0.5;
00357   ocm.orientation.y = 0.5;
00358   ocm.orientation.z = 0.5;
00359   ocm.orientation.w = 0.5;
00360   ocm.absolute_x_axis_tolerance = 0.01;
00361   ocm.absolute_y_axis_tolerance = 0.01;
00362   ocm.absolute_z_axis_tolerance = 0.01;
00363   ocm.weight = 1.0;
00364 
00365   EXPECT_TRUE(oc.configure(ocm, tf));
00366 
00367   bool p1 = oc.decide(ks).satisfied;
00368   EXPECT_FALSE(p1);
00369 
00370   ocm.header.frame_id = kmodel->getModelFrame();
00371   EXPECT_TRUE(oc.configure(ocm, tf));
00372 
00373   constraint_samplers::IKConstraintSampler iks(ps, "right_arm");
00374   EXPECT_TRUE(iks.configure(constraint_samplers::IKSamplingPose(oc)));
00375   for (int t = 0; t < 100; ++t)
00376   {
00377     ks.update();
00378     EXPECT_TRUE(iks.sample(ks, ks_const, 100));
00379     EXPECT_TRUE(oc.decide(ks).satisfied);
00380   }
00381 }
00382 
00383 TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerValid)
00384 {
00385   robot_state::RobotState ks(kmodel);
00386   ks.setToDefaultValues();
00387   ks.update();
00388   robot_state::RobotState ks_const(kmodel);
00389   ks_const.setToDefaultValues();
00390   ks_const.update();
00391 
00392   robot_state::Transforms& tf = ps->getTransformsNonConst();
00393 
00394   kinematic_constraints::PositionConstraint pc(kmodel);
00395   moveit_msgs::PositionConstraint pcm;
00396 
00397   pcm.link_name = "l_wrist_roll_link";
00398   pcm.target_point_offset.x = 0;
00399   pcm.target_point_offset.y = 0;
00400   pcm.target_point_offset.z = 0;
00401   pcm.constraint_region.primitives.resize(1);
00402   pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00403   pcm.constraint_region.primitives[0].dimensions.resize(1);
00404   pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00405 
00406   pcm.header.frame_id = kmodel->getModelFrame();
00407 
00408   pcm.constraint_region.primitive_poses.resize(1);
00409   pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00410   pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00411   pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00412   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00413   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00414   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00415   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00416   pcm.weight = 1.0;
00417 
00418   EXPECT_TRUE(pc.configure(pcm, tf));
00419 
00420   kinematic_constraints::OrientationConstraint oc(kmodel);
00421   moveit_msgs::OrientationConstraint ocm;
00422 
00423   ocm.link_name = "l_wrist_roll_link";
00424   ocm.header.frame_id = kmodel->getModelFrame();
00425   ocm.orientation.x = 0.0;
00426   ocm.orientation.y = 0.0;
00427   ocm.orientation.z = 0.0;
00428   ocm.orientation.w = 1.0;
00429   ocm.absolute_x_axis_tolerance = 0.2;
00430   ocm.absolute_y_axis_tolerance = 0.1;
00431   ocm.absolute_z_axis_tolerance = 0.4;
00432   ocm.weight = 1.0;
00433 
00434   EXPECT_TRUE(oc.configure(ocm, tf));
00435 
00436   constraint_samplers::IKConstraintSampler iks1(ps, "left_arm");
00437   EXPECT_TRUE(iks1.configure(constraint_samplers::IKSamplingPose(pc, oc)));
00438   for (int t = 0; t < 100; ++t)
00439   {
00440     EXPECT_TRUE(iks1.sample(ks, ks_const, 100));
00441     EXPECT_TRUE(pc.decide(ks).satisfied);
00442     EXPECT_TRUE(oc.decide(ks).satisfied);
00443   }
00444 
00445   constraint_samplers::IKConstraintSampler iks2(ps, "left_arm");
00446   EXPECT_TRUE(iks2.configure(constraint_samplers::IKSamplingPose(pc)));
00447   for (int t = 0; t < 100; ++t)
00448   {
00449     EXPECT_TRUE(iks2.sample(ks, ks_const, 100));
00450     EXPECT_TRUE(pc.decide(ks).satisfied);
00451   }
00452 
00453   constraint_samplers::IKConstraintSampler iks3(ps, "left_arm");
00454   EXPECT_TRUE(iks3.configure(constraint_samplers::IKSamplingPose(oc)));
00455   for (int t = 0; t < 100; ++t)
00456   {
00457     EXPECT_TRUE(iks3.sample(ks, ks_const, 100));
00458     EXPECT_TRUE(oc.decide(ks).satisfied);
00459   }
00460 }
00461 
00462 TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
00463 {
00464   robot_state::RobotState ks(kmodel);
00465   ks.setToDefaultValues();
00466   ks.update();
00467 
00468   robot_state::RobotState ks_const(kmodel);
00469   ks_const.setToDefaultValues();
00470   ks_const.update();
00471 
00472   robot_state::Transforms& tf = ps->getTransformsNonConst();
00473 
00474   kinematic_constraints::JointConstraint jc1(kmodel);
00475 
00476   std::map<std::string, double> state_values;
00477 
00478   moveit_msgs::JointConstraint torso_constraint;
00479   torso_constraint.joint_name = "torso_lift_joint";
00480   torso_constraint.position = ks.getVariablePosition("torso_lift_joint");
00481   torso_constraint.tolerance_above = 0.01;
00482   torso_constraint.tolerance_below = 0.01;
00483   torso_constraint.weight = 1.0;
00484   EXPECT_TRUE(jc1.configure(torso_constraint));
00485 
00486   kinematic_constraints::JointConstraint jc2(kmodel);
00487   moveit_msgs::JointConstraint jcm2;
00488   jcm2.joint_name = "r_elbow_flex_joint";
00489   jcm2.position = ks.getVariablePosition("r_elbow_flex_joint");
00490   jcm2.tolerance_above = 0.01;
00491   jcm2.tolerance_below = 0.01;
00492   jcm2.weight = 1.0;
00493   EXPECT_TRUE(jc2.configure(jcm2));
00494 
00495   moveit_msgs::PositionConstraint pcm;
00496 
00497   pcm.link_name = "l_wrist_roll_link";
00498   pcm.target_point_offset.x = 0;
00499   pcm.target_point_offset.y = 0;
00500   pcm.target_point_offset.z = 0;
00501   pcm.constraint_region.primitives.resize(1);
00502   pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00503   pcm.constraint_region.primitives[0].dimensions.resize(1);
00504   pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00505 
00506   pcm.constraint_region.primitive_poses.resize(1);
00507   pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00508   pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00509   pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00510   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00511   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00512   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00513   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00514   pcm.weight = 1.0;
00515 
00516   pcm.header.frame_id = kmodel->getModelFrame();
00517 
00518   moveit_msgs::OrientationConstraint ocm;
00519 
00520   ocm.link_name = "l_wrist_roll_link";
00521   ocm.header.frame_id = kmodel->getModelFrame();
00522   ocm.orientation.x = 0.0;
00523   ocm.orientation.y = 0.0;
00524   ocm.orientation.z = 0.0;
00525   ocm.orientation.w = 1.0;
00526   ocm.absolute_x_axis_tolerance = 0.2;
00527   ocm.absolute_y_axis_tolerance = 0.1;
00528   ocm.absolute_z_axis_tolerance = 0.4;
00529   ocm.weight = 1.0;
00530 
00531   std::vector<kinematic_constraints::JointConstraint> js;
00532   js.push_back(jc1);
00533 
00534   constraint_samplers::JointConstraintSamplerPtr jcsp(new constraint_samplers::JointConstraintSampler(ps, "arms_and_"
00535                                                                                                           "torso"));
00536   EXPECT_TRUE(jcsp->configure(js));
00537 
00538   std::vector<kinematic_constraints::JointConstraint> js2;
00539   js2.push_back(jc2);
00540 
00541   constraint_samplers::JointConstraintSamplerPtr jcsp2(new constraint_samplers::JointConstraintSampler(ps, "arms"));
00542   EXPECT_TRUE(jcsp2->configure(js2));
00543 
00544   kinematic_constraints::PositionConstraint pc(kmodel);
00545   EXPECT_TRUE(pc.configure(pcm, tf));
00546 
00547   kinematic_constraints::OrientationConstraint oc(kmodel);
00548   EXPECT_TRUE(oc.configure(ocm, tf));
00549 
00550   constraint_samplers::IKConstraintSamplerPtr iksp(new constraint_samplers::IKConstraintSampler(ps, "left_arm"));
00551   EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
00552   EXPECT_TRUE(iksp->isValid());
00553 
00554   std::vector<constraint_samplers::ConstraintSamplerPtr> cspv;
00555   cspv.push_back(jcsp2);
00556   cspv.push_back(iksp);
00557   cspv.push_back(jcsp);
00558 
00559   constraint_samplers::UnionConstraintSampler ucs(ps, "arms_and_torso", cspv);
00560 
00561   // should have reordered to place whole body first
00562   constraint_samplers::JointConstraintSampler* jcs =
00563       dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[0].get());
00564   EXPECT_TRUE(jcs);
00565   EXPECT_EQ(jcs->getJointModelGroup()->getName(), "arms_and_torso");
00566 
00567   constraint_samplers::JointConstraintSampler* jcs2 =
00568       dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs.getSamplers()[1].get());
00569   EXPECT_TRUE(jcs2);
00570   EXPECT_EQ(jcs2->getJointModelGroup()->getName(), "arms");
00571 
00572   for (int t = 0; t < 100; ++t)
00573   {
00574     EXPECT_TRUE(ucs.sample(ks, ks_const, 100));
00575     ks.update();
00576     ks.updateLinkTransforms();  // Returned samples have dirty link transforms.
00577     ks_const.update();
00578     EXPECT_TRUE(jc1.decide(ks).satisfied);
00579     EXPECT_TRUE(jc2.decide(ks).satisfied);
00580     EXPECT_TRUE(pc.decide(ks).satisfied);
00581   }
00582 
00583   // now we add a position constraint on right arm
00584   pcm.link_name = "r_wrist_roll_link";
00585   ocm.link_name = "r_wrist_roll_link";
00586   cspv.clear();
00587 
00588   kinematic_constraints::PositionConstraint pc2(kmodel);
00589   EXPECT_TRUE(pc2.configure(pcm, tf));
00590 
00591   kinematic_constraints::OrientationConstraint oc2(kmodel);
00592   EXPECT_TRUE(oc2.configure(ocm, tf));
00593 
00594   constraint_samplers::IKConstraintSamplerPtr iksp2(new constraint_samplers::IKConstraintSampler(ps, "right_arm"));
00595   EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
00596   EXPECT_TRUE(iksp2->isValid());
00597 
00598   // totally disjoint, so should break ties based on alphabetical order
00599   cspv.clear();
00600   cspv.push_back(iksp2);
00601   cspv.push_back(iksp);
00602 
00603   constraint_samplers::UnionConstraintSampler ucs2(ps, "arms_and_torso", cspv);
00604 
00605   constraint_samplers::IKConstraintSampler* ikcs_test =
00606       dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs2.getSamplers()[0].get());
00607   ASSERT_TRUE(ikcs_test);
00608   EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "left_arm");
00609 
00610   // now we make left depends on right, right should stay first
00611   pcm.link_name = "l_wrist_roll_link";
00612   ocm.link_name = "l_wrist_roll_link";
00613   pcm.header.frame_id = "r_wrist_roll_link";
00614   ocm.header.frame_id = "r_wrist_roll_link";
00615   EXPECT_TRUE(pc.configure(pcm, tf));
00616   EXPECT_TRUE(oc.configure(ocm, tf));
00617   ASSERT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
00618 
00619   cspv.clear();
00620   cspv.push_back(iksp2);
00621   cspv.push_back(iksp);
00622 
00623   constraint_samplers::UnionConstraintSampler ucs3(ps, "arms_and_torso", cspv);
00624 
00625   ikcs_test = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs3.getSamplers()[0].get());
00626   EXPECT_TRUE(ikcs_test);
00627   EXPECT_EQ(ikcs_test->getJointModelGroup()->getName(), "right_arm");
00628 }
00629 
00630 TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager)
00631 {
00632   robot_state::RobotState ks(kmodel);
00633   ks.setToDefaultValues();
00634   ks.update();
00635   robot_state::RobotState ks_const(kmodel);
00636   ks_const.setToDefaultValues();
00637   ks_const.update();
00638 
00639   kinematic_constraints::PositionConstraint pc(kmodel);
00640 
00641   moveit_msgs::PositionConstraint pcm;
00642   pcm.link_name = "l_wrist_roll_link";
00643   pcm.target_point_offset.x = 0;
00644   pcm.target_point_offset.y = 0;
00645   pcm.target_point_offset.z = 0;
00646   pcm.constraint_region.primitives.resize(1);
00647   pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00648   pcm.constraint_region.primitives[0].dimensions.resize(1);
00649   pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
00650 
00651   pcm.header.frame_id = kmodel->getModelFrame();
00652 
00653   pcm.constraint_region.primitive_poses.resize(1);
00654   pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00655   pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00656   pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00657   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00658   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00659   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00660   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00661   pcm.weight = 1.0;
00662 
00663   moveit_msgs::OrientationConstraint ocm;
00664 
00665   ocm.link_name = "l_wrist_roll_link";
00666   ocm.header.frame_id = kmodel->getModelFrame();
00667   ocm.orientation.x = 0.0;
00668   ocm.orientation.y = 0.0;
00669   ocm.orientation.z = 0.0;
00670   ocm.orientation.w = 1.0;
00671   ocm.absolute_x_axis_tolerance = 0.2;
00672   ocm.absolute_y_axis_tolerance = 0.1;
00673   ocm.absolute_z_axis_tolerance = 0.4;
00674   ocm.weight = 1.0;
00675 
00676   // test the automatic construction of constraint sampler
00677   moveit_msgs::Constraints c;
00678   c.position_constraints.push_back(pcm);
00679   c.orientation_constraints.push_back(ocm);
00680 
00681   constraint_samplers::ConstraintSamplerPtr s =
00682       constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", c);
00683   EXPECT_TRUE(s.get() != NULL);
00684   constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
00685   ASSERT_TRUE(iks);
00686   ASSERT_TRUE(iks->getPositionConstraint());
00687   ASSERT_TRUE(iks->getOrientationConstraint());
00688 
00689   static const int NT = 100;
00690   int succ = 0;
00691   for (int t = 0; t < NT; ++t)
00692   {
00693     EXPECT_TRUE(s->sample(ks, ks_const, 100));
00694     EXPECT_TRUE(iks->getPositionConstraint()->decide(ks).satisfied);
00695     EXPECT_TRUE(iks->getOrientationConstraint()->decide(ks).satisfied);
00696     if (s->sample(ks, ks_const, 1))
00697       succ++;
00698   }
00699   ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf",
00700            (double)succ / (double)NT);
00701 
00702   // add additional ocm with smaller volume
00703   ocm.absolute_x_axis_tolerance = 0.1;
00704 
00705   c.orientation_constraints.push_back(ocm);
00706 
00707   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", c);
00708   EXPECT_TRUE(s.get() != NULL);
00709 
00710   iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
00711   ASSERT_TRUE(iks);
00712   ASSERT_TRUE(iks->getOrientationConstraint());
00713   EXPECT_NEAR(iks->getOrientationConstraint()->getXAxisTolerance(), .1, .0001);
00714 }
00715 
00716 TEST_F(LoadPlanningModelsPr2, JointVersusPoseConstraintSamplerManager)
00717 {
00718   robot_state::RobotState ks(kmodel);
00719   ks.setToDefaultValues();
00720   ks.update();
00721 
00722   moveit_msgs::Constraints con;
00723   con.joint_constraints.resize(1);
00724 
00725   con.joint_constraints[0].joint_name = "l_shoulder_pan_joint";
00726   con.joint_constraints[0].position = 0.54;
00727   con.joint_constraints[0].tolerance_above = 0.01;
00728   con.joint_constraints[0].tolerance_below = 0.01;
00729   con.joint_constraints[0].weight = 1.0;
00730 
00731   constraint_samplers::ConstraintSamplerPtr s =
00732       constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "right_arm", con);
00733   EXPECT_FALSE(s);
00734   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00735   EXPECT_TRUE(s);
00736 
00737   con.joint_constraints.resize(7);
00738 
00739   con.joint_constraints[1].joint_name = "l_shoulder_lift_joint";
00740   con.joint_constraints[1].position = 0.54;
00741   con.joint_constraints[1].tolerance_above = 0.01;
00742   con.joint_constraints[1].tolerance_below = 0.01;
00743   con.joint_constraints[1].weight = 1.0;
00744 
00745   con.joint_constraints[2].joint_name = "l_upper_arm_roll_joint";
00746   con.joint_constraints[2].position = 0.54;
00747   con.joint_constraints[2].tolerance_above = 0.01;
00748   con.joint_constraints[2].tolerance_below = 0.01;
00749   con.joint_constraints[2].weight = 1.0;
00750 
00751   con.joint_constraints[3].joint_name = "l_elbow_flex_joint";
00752   con.joint_constraints[3].position = -0.54;
00753   con.joint_constraints[3].tolerance_above = 0.01;
00754   con.joint_constraints[3].tolerance_below = 0.01;
00755   con.joint_constraints[3].weight = 1.0;
00756 
00757   con.joint_constraints[4].joint_name = "l_forearm_roll_joint";
00758   con.joint_constraints[4].position = 0.54;
00759   con.joint_constraints[4].tolerance_above = 0.01;
00760   con.joint_constraints[4].tolerance_below = 0.01;
00761   con.joint_constraints[4].weight = 1.0;
00762 
00763   con.joint_constraints[5].joint_name = "l_wrist_flex_joint";
00764   con.joint_constraints[5].position = -0.54;
00765   con.joint_constraints[5].tolerance_above = 0.05;
00766   con.joint_constraints[5].tolerance_below = 0.05;
00767   con.joint_constraints[5].weight = 1.0;
00768 
00769   // an extra constraint on one link, but this shouldn't change anything
00770   con.joint_constraints[6].joint_name = "l_wrist_flex_joint";
00771   con.joint_constraints[6].position = -0.56;
00772   con.joint_constraints[6].tolerance_above = 0.01;
00773   con.joint_constraints[6].tolerance_below = 0.01;
00774   con.joint_constraints[6].weight = 1.0;
00775 
00776   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00777   EXPECT_TRUE(s);
00778 
00779   con.position_constraints.resize(1);
00780 
00781   // intentionally making wrong wrist
00782   con.position_constraints[0].link_name = "r_wrist_roll_link";
00783   con.position_constraints[0].target_point_offset.x = 0;
00784   con.position_constraints[0].target_point_offset.y = 0;
00785   con.position_constraints[0].target_point_offset.z = 0;
00786   con.position_constraints[0].constraint_region.primitives.resize(1);
00787   con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00788   con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
00789   con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
00790 
00791   con.position_constraints[0].header.frame_id = kmodel->getModelFrame();
00792 
00793   con.position_constraints[0].constraint_region.primitive_poses.resize(1);
00794   con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
00795   con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
00796   con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
00797   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
00798   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
00799   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
00800   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
00801   con.position_constraints[0].weight = 1.0;
00802 
00803   // this still works, but we should get a JointConstraintSampler
00804   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00805   EXPECT_TRUE(s);
00806   constraint_samplers::JointConstraintSampler* jcs =
00807       dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
00808   EXPECT_TRUE(jcs);
00809 
00810   con.position_constraints[0].link_name = "l_wrist_roll_link";
00811   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00812   EXPECT_TRUE(s);
00813   jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
00814   EXPECT_FALSE(jcs);
00815   constraint_samplers::IKConstraintSampler* iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(s.get());
00816   EXPECT_FALSE(iks);
00817 
00818   // we should get a union constraint sampler
00819   constraint_samplers::UnionConstraintSampler* ucs =
00820       dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00821   EXPECT_TRUE(ucs);
00822 
00823   con.orientation_constraints.resize(1);
00824 
00825   // again, screwing this up intentionally
00826   con.orientation_constraints[0].link_name = "r_wrist_roll_link";
00827   con.orientation_constraints[0].header.frame_id = kmodel->getModelFrame();
00828   con.orientation_constraints[0].orientation.x = 0.0;
00829   con.orientation_constraints[0].orientation.y = 0.0;
00830   con.orientation_constraints[0].orientation.z = 0.0;
00831   con.orientation_constraints[0].orientation.w = 1.0;
00832   con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
00833   con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
00834   con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
00835   con.orientation_constraints[0].weight = 1.0;
00836 
00837   // we still get an IK sampler with just the position constraint
00838   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00839   EXPECT_TRUE(s);
00840   ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00841   ASSERT_TRUE(ucs);
00842   jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(ucs->getSamplers()[0].get());
00843   iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00844 
00845   ASSERT_TRUE(iks);
00846   ASSERT_TRUE(jcs);
00847   EXPECT_TRUE(iks->getPositionConstraint());
00848   EXPECT_FALSE(iks->getOrientationConstraint());
00849 
00850   con.orientation_constraints[0].link_name = "l_wrist_roll_link";
00851 
00852   // now they both are good
00853   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00854   EXPECT_TRUE(s);
00855   ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00856   iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00857   ASSERT_TRUE(iks);
00858   EXPECT_TRUE(iks->getPositionConstraint());
00859   EXPECT_TRUE(iks->getOrientationConstraint());
00860 
00861   // now just the orientation constraint is good
00862   con.position_constraints[0].link_name = "r_wrist_roll_link";
00863   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00864   ASSERT_TRUE(s);
00865   ucs = dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00866   iks = dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00867   ASSERT_TRUE(iks);
00868   EXPECT_FALSE(iks->getPositionConstraint());
00869   EXPECT_TRUE(iks->getOrientationConstraint());
00870 
00871   // now if we constraint all the joints, we get a joint constraint sampler
00872   con.joint_constraints.resize(8);
00873   con.joint_constraints[7].joint_name = "l_wrist_roll_joint";
00874   con.joint_constraints[7].position = 0.54;
00875   con.joint_constraints[7].tolerance_above = 0.01;
00876   con.joint_constraints[7].tolerance_below = 0.01;
00877   con.joint_constraints[7].weight = 1.0;
00878 
00879   s = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "left_arm", con);
00880   EXPECT_TRUE(s);
00881   jcs = dynamic_cast<constraint_samplers::JointConstraintSampler*>(s.get());
00882   ASSERT_TRUE(jcs);
00883 }
00884 
00885 TEST_F(LoadPlanningModelsPr2, MixedJointAndIkSamplerManager)
00886 {
00887   robot_state::RobotState ks(kmodel);
00888   ks.setToDefaultValues();
00889   ks.update();
00890   robot_state::RobotState ks_const(kmodel);
00891   ks_const.setToDefaultValues();
00892   ks_const.update();
00893 
00894   moveit_msgs::Constraints con;
00895   con.joint_constraints.resize(1);
00896 
00897   con.joint_constraints[0].joint_name = "torso_lift_joint";
00898   con.joint_constraints[0].position = ks.getVariablePosition("torso_lift_joint");
00899   con.joint_constraints[0].tolerance_above = 0.01;
00900   con.joint_constraints[0].tolerance_below = 0.01;
00901   con.joint_constraints[0].weight = 1.0;
00902 
00903   kinematic_constraints::JointConstraint jc(kmodel);
00904   EXPECT_TRUE(jc.configure(con.joint_constraints[0]));
00905 
00906   con.position_constraints.resize(1);
00907 
00908   con.position_constraints[0].link_name = "l_wrist_roll_link";
00909   con.position_constraints[0].target_point_offset.x = 0;
00910   con.position_constraints[0].target_point_offset.y = 0;
00911   con.position_constraints[0].target_point_offset.z = 0;
00912   con.position_constraints[0].constraint_region.primitives.resize(1);
00913   con.position_constraints[0].constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00914   con.position_constraints[0].constraint_region.primitives[0].dimensions.resize(1);
00915   con.position_constraints[0].constraint_region.primitives[0].dimensions[0] = 0.001;
00916 
00917   con.position_constraints[0].constraint_region.primitive_poses.resize(1);
00918   con.position_constraints[0].constraint_region.primitive_poses[0].position.x = 0.55;
00919   con.position_constraints[0].constraint_region.primitive_poses[0].position.y = 0.2;
00920   con.position_constraints[0].constraint_region.primitive_poses[0].position.z = 1.25;
00921   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0;
00922   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0;
00923   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0;
00924   con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0;
00925   con.position_constraints[0].weight = 1.0;
00926 
00927   con.position_constraints[0].header.frame_id = kmodel->getModelFrame();
00928 
00929   con.orientation_constraints.resize(1);
00930   con.orientation_constraints[0].link_name = "l_wrist_roll_link";
00931   con.orientation_constraints[0].header.frame_id = kmodel->getModelFrame();
00932   con.orientation_constraints[0].orientation.x = 0.0;
00933   con.orientation_constraints[0].orientation.y = 0.0;
00934   con.orientation_constraints[0].orientation.z = 0.0;
00935   con.orientation_constraints[0].orientation.w = 1.0;
00936   con.orientation_constraints[0].absolute_x_axis_tolerance = 0.2;
00937   con.orientation_constraints[0].absolute_y_axis_tolerance = 0.1;
00938   con.orientation_constraints[0].absolute_z_axis_tolerance = 0.4;
00939   con.orientation_constraints[0].weight = 1.0;
00940 
00941   constraint_samplers::ConstraintSamplerPtr s =
00942       constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms_and_torso", con);
00943 
00944   constraint_samplers::UnionConstraintSampler* ucs =
00945       dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
00946   ASSERT_TRUE(ucs);
00947 
00948   constraint_samplers::IKConstraintSampler* ikcs_test =
00949       dynamic_cast<constraint_samplers::IKConstraintSampler*>(ucs->getSamplers()[1].get());
00950   ASSERT_TRUE(ikcs_test);
00951 
00952   for (int t = 0; t < 1; ++t)
00953   {
00954     EXPECT_TRUE(s->sample(ks, ks_const, 100));
00955     EXPECT_TRUE(jc.decide(ks).satisfied);
00956     EXPECT_TRUE(ikcs_test->getPositionConstraint()->decide(ks).satisfied);
00957     EXPECT_TRUE(ikcs_test->getOrientationConstraint()->decide(ks).satisfied);
00958   }
00959 }
00960 
00961 TEST_F(LoadPlanningModelsPr2, SubgroupJointConstraintsSamplerManager)
00962 {
00963   robot_state::RobotState ks(kmodel);
00964   ks.setToDefaultValues();
00965   ks.update();
00966   robot_state::RobotState ks_const(kmodel);
00967   ks_const.setToDefaultValues();
00968   ks_const.update();
00969 
00970   kinematic_constraints::JointConstraint jc1(kmodel);
00971   moveit_msgs::JointConstraint jcm1;
00972   jcm1.joint_name = "head_pan_joint";
00973   jcm1.position = 0.42;
00974   jcm1.tolerance_above = 0.01;
00975   jcm1.tolerance_below = 0.05;
00976   jcm1.weight = 1.0;
00977   EXPECT_TRUE(jc1.configure(jcm1));
00978 
00979   kinematic_constraints::JointConstraint jc2(kmodel);
00980   moveit_msgs::JointConstraint jcm2;
00981   jcm2.joint_name = "l_shoulder_pan_joint";
00982   jcm2.position = 0.9;
00983   jcm2.tolerance_above = 0.1;
00984   jcm2.tolerance_below = 0.05;
00985   jcm2.weight = 1.0;
00986   EXPECT_TRUE(jc2.configure(jcm2));
00987 
00988   kinematic_constraints::JointConstraint jc3(kmodel);
00989   moveit_msgs::JointConstraint jcm3;
00990   jcm3.joint_name = "r_wrist_roll_joint";
00991   jcm3.position = 0.7;
00992   jcm3.tolerance_above = 0.14;
00993   jcm3.tolerance_below = 0.005;
00994   jcm3.weight = 1.0;
00995   EXPECT_TRUE(jc3.configure(jcm3));
00996 
00997   kinematic_constraints::JointConstraint jc4(kmodel);
00998   moveit_msgs::JointConstraint jcm4;
00999   jcm4.joint_name = "torso_lift_joint";
01000   jcm4.position = 0.2;
01001   jcm4.tolerance_above = 0.09;
01002   jcm4.tolerance_below = 0.01;
01003   jcm4.weight = 1.0;
01004   EXPECT_TRUE(jc4.configure(jcm4));
01005 
01006   std::vector<kinematic_constraints::JointConstraint> js;
01007   js.push_back(jc1);
01008   js.push_back(jc2);
01009   js.push_back(jc3);
01010   js.push_back(jc4);
01011 
01012   constraint_samplers::JointConstraintSampler jcs(ps, "arms");
01013   jcs.configure(js);
01014   EXPECT_EQ(jcs.getConstrainedJointCount(), 2);
01015   EXPECT_EQ(jcs.getUnconstrainedJointCount(), 12);
01016 
01017   for (int t = 0; t < 100; ++t)
01018   {
01019     EXPECT_TRUE(jcs.sample(ks, ks_const, 1));
01020     EXPECT_TRUE(jc2.decide(ks).satisfied);
01021     EXPECT_TRUE(jc3.decide(ks).satisfied);
01022   }
01023 
01024   // test the automatic construction of constraint sampler
01025   moveit_msgs::Constraints c;
01026 
01027   // no constraints should give no sampler
01028   constraint_samplers::ConstraintSamplerPtr s0 =
01029       constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01030   EXPECT_TRUE(s0.get() == NULL);
01031 
01032   // add the constraints
01033   c.joint_constraints.push_back(jcm1);
01034   c.joint_constraints.push_back(jcm2);
01035   c.joint_constraints.push_back(jcm3);
01036   c.joint_constraints.push_back(jcm4);
01037 
01038   constraint_samplers::ConstraintSamplerPtr s =
01039       constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01040   EXPECT_TRUE(s.get() != NULL);
01041 
01042   // test the generated sampler
01043   for (int t = 0; t < 1000; ++t)
01044   {
01045     EXPECT_TRUE(s->sample(ks, ks_const, 1));
01046     EXPECT_TRUE(jc2.decide(ks).satisfied);
01047     EXPECT_TRUE(jc3.decide(ks).satisfied);
01048   }
01049 }
01050 
01051 TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
01052 {
01053   moveit_msgs::Constraints c;
01054 
01055   moveit_msgs::PositionConstraint pcm;
01056   pcm.link_name = "l_wrist_roll_link";
01057   pcm.target_point_offset.x = 0;
01058   pcm.target_point_offset.y = 0;
01059   pcm.target_point_offset.z = 0;
01060 
01061   pcm.constraint_region.primitives.resize(1);
01062   pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
01063   pcm.constraint_region.primitives[0].dimensions.resize(1);
01064   pcm.constraint_region.primitives[0].dimensions[0] = 0.001;
01065 
01066   pcm.header.frame_id = kmodel->getModelFrame();
01067 
01068   pcm.constraint_region.primitive_poses.resize(1);
01069   pcm.constraint_region.primitive_poses[0].position.x = 0.55;
01070   pcm.constraint_region.primitive_poses[0].position.y = 0.2;
01071   pcm.constraint_region.primitive_poses[0].position.z = 1.25;
01072   pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
01073   pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
01074   pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
01075   pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
01076   pcm.weight = 1.0;
01077   c.position_constraints.push_back(pcm);
01078 
01079   moveit_msgs::OrientationConstraint ocm;
01080   ocm.link_name = "l_wrist_roll_link";
01081   ocm.header.frame_id = kmodel->getModelFrame();
01082   ocm.orientation.x = 0.0;
01083   ocm.orientation.y = 0.0;
01084   ocm.orientation.z = 0.0;
01085   ocm.orientation.w = 1.0;
01086   ocm.absolute_x_axis_tolerance = 0.2;
01087   ocm.absolute_y_axis_tolerance = 0.1;
01088   ocm.absolute_z_axis_tolerance = 0.4;
01089   ocm.weight = 1.0;
01090   c.orientation_constraints.push_back(ocm);
01091 
01092   ocm.link_name = "r_wrist_roll_link";
01093   ocm.header.frame_id = kmodel->getModelFrame();
01094   ocm.orientation.x = 0.0;
01095   ocm.orientation.y = 0.0;
01096   ocm.orientation.z = 0.0;
01097   ocm.orientation.w = 1.0;
01098   ocm.absolute_x_axis_tolerance = 0.01;
01099   ocm.absolute_y_axis_tolerance = 0.01;
01100   ocm.absolute_z_axis_tolerance = 0.01;
01101   ocm.weight = 1.0;
01102   c.orientation_constraints.push_back(ocm);
01103 
01104   robot_state::Transforms& tf = ps->getTransformsNonConst();
01105   constraint_samplers::ConstraintSamplerPtr s =
01106       constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01107   EXPECT_TRUE(s);
01108   constraint_samplers::UnionConstraintSampler* ucs =
01109       dynamic_cast<constraint_samplers::UnionConstraintSampler*>(s.get());
01110   EXPECT_TRUE(ucs);
01111 
01112   kinematic_constraints::KinematicConstraintSet kset(kmodel);
01113   kset.add(c, tf);
01114 
01115   robot_state::RobotState ks(kmodel);
01116   ks.setToDefaultValues();
01117   ks.update();
01118   robot_state::RobotState ks_const(kmodel);
01119   ks_const.setToDefaultValues();
01120   ks_const.update();
01121 
01122   static const int NT = 100;
01123   int succ = 0;
01124   for (int t = 0; t < NT; ++t)
01125   {
01126     EXPECT_TRUE(s->sample(ks, ks_const, 1000));
01127     EXPECT_TRUE(kset.decide(ks).satisfied);
01128     if (s->sample(ks, ks_const, 1))
01129       succ++;
01130   }
01131   logInform("Success rate for IK Constraint Sampler with position & orientation constraints for both arms: %lf",
01132             (double)succ / (double)NT);
01133 }
01134 
01135 int main(int argc, char** argv)
01136 {
01137   testing::InitGoogleTest(&argc, argv);
01138   ros::Time::init();
01139   return RUN_ALL_TESTS();
01140 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49