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


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