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


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