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 
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   //leaving off joint name
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   //no valid constraints
00174   EXPECT_FALSE(jcs.configure(js));
00175 
00176   //testing that this does the right thing
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   //redoing the configure leads to 6 unconstrained variables as well
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   //creating a constraint that conflicts with the other (leaves no sampleable region)
00207   EXPECT_FALSE(jcs.configure(js));
00208   EXPECT_FALSE(jcs.sample(ks, ks_const, 1));
00209 
00210   //we can't sample for a different group
00211   constraint_samplers::JointConstraintSampler jcs2(ps, "arms");
00212   jcs2.configure(js);
00213   EXPECT_FALSE(jcs2.sample(ks, ks_const, 1));
00214 
00215   //not ok to not have any references to joints in this group in the constraints
00216   constraint_samplers::JointConstraintSampler jcs3(ps, "left_arm");
00217   EXPECT_FALSE(jcs3.configure(js));
00218 
00219   //testing that the most restrictive bounds are used
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   //should always be within narrower constraints
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   //too narrow range outside of joint limits
00243   js.clear();
00244 
00245   jcm1.position = -3.1;
00246   jcm1.tolerance_above = .05;
00247   jcm1.tolerance_below = .05;
00248 
00249   //the joint configuration corrects this
00250   EXPECT_TRUE(jc1.configure(jcm1));
00251   js.push_back(jc1);
00252   EXPECT_TRUE(jcs.configure(js));
00253 
00254   //partially overlapping regions will also work
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   //leads to a min and max of .4, so all samples should be exactly .4
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   //this leads to a larger sampleable region
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   //ik link not in this group
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   //not the ik link
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   //solver for base doesn't cover group
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   //shouldn't work as no direct constraint solver
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   //  ks.getStateValues(state_values);
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   //should have reordered to place whole body first
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   //now we add a position constraint on right arm
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   //totally disjoint, so should break ties based on alphabetical order
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   //now we make left depends on right, right should stay first
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   // test the automatic construction of constraint sampler
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   //add additional ocm with smaller volume
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   //an extra constraint on one link, but this shouldn't change anything
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   //intentionally making wrong wrist
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   //this still works, but we should get a JointConstraintSampler
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   //we should get a union constraint sampler
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   //again, screwing this up intentionally
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   //we still get an IK sampler with just the position constraint
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   //now they both are good
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   //now just the orientation constraint is good
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   //now if we constraint all the joints, we get a joint constraint sampler
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   //  ks.getStateValues(state_values);
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   // test the automatic construction of constraint sampler
01028   moveit_msgs::Constraints c;
01029 
01030   // no constraints should give no sampler
01031   constraint_samplers::ConstraintSamplerPtr s0 = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(ps, "arms", c);
01032   EXPECT_TRUE(s0.get() == NULL);
01033 
01034   // add the constraints
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   // test the generated sampler
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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53