test_constraints.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan, E. Gil Jones */

#include <moveit/kinematic_constraints/kinematic_constraint.h>
#include <gtest/gtest.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/filesystem/path.hpp>
#include <moveit_resources/config.h>

class LoadPlanningModelsPr2 : public testing::Test
{
protected:
  virtual void SetUp()
  {
    boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);

    srdf_model.reset(new srdf::Model());
    std::string xml_string;
    std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
    if (xml_file.is_open())
    {
      while (xml_file.good())
      {
        std::string line;
        std::getline(xml_file, line);
        xml_string += (line + "\n");
      }
      xml_file.close();
      urdf_model = urdf::parseURDF(xml_string);
    }
    else
    {
      FAIL() << "Failed to find robot.xml";
    }
    srdf_model->initFile(*urdf_model, (res_path / "pr2_description/srdf/robot.xml").string());
    kmodel.reset(new robot_model::RobotModel(urdf_model, srdf_model));
  };

  virtual void TearDown()
  {
  }

protected:
  urdf::ModelInterfaceSharedPtr urdf_model;
  srdf::ModelSharedPtr srdf_model;
  robot_model::RobotModelPtr kmodel;
};

TEST_F(LoadPlanningModelsPr2, JointConstraintsSimple)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::JointConstraint jc(kmodel);
  moveit_msgs::JointConstraint jcm;
  jcm.joint_name = "head_pan_joint";
  jcm.position = 0.4;
  jcm.tolerance_above = 0.1;
  jcm.tolerance_below = 0.05;

  EXPECT_TRUE(jc.configure(jcm));
  // weight should have been changed to 1.0
  EXPECT_NEAR(jc.getConstraintWeight(), 1.0, std::numeric_limits<double>::epsilon());

  // tests that the default state is outside the bounds
  // given that the default state is at 0.0
  EXPECT_TRUE(jc.configure(jcm));
  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
  EXPECT_FALSE(p1.satisfied);
  EXPECT_NEAR(p1.distance, jcm.position, 1e-6);

  // tests that when we set the state within the bounds
  // the constraint is satisfied
  double jval = 0.41;
  ks.setJointPositions(jcm.joint_name, &jval);
  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
  EXPECT_TRUE(p2.satisfied);
  EXPECT_NEAR(p2.distance, 0.01, 1e-6);

  // exactly equal to the low bound is fine too
  jval = 0.35;
  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // and so is less than epsilon when there's no other source of error
  //    jvals[jcm.joint_name] = 0.35-std::numeric_limits<double>::epsilon();
  jval = 0.35 - std::numeric_limits<double>::epsilon();

  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // but this is too much
  jval = 0.35 - 3 * std::numeric_limits<double>::epsilon();
  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // negative value makes configuration fail
  jcm.tolerance_below = -0.05;
  EXPECT_FALSE(jc.configure(jcm));

  jcm.tolerance_below = 0.05;
  EXPECT_TRUE(jc.configure(jcm));

  // still satisfied at a slightly different state
  jval = 0.46;
  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // still satisfied at a slightly different state
  jval = 0.501;
  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // still satisfied at a slightly different state
  jval = 0.39;
  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // outside the bounds
  jval = 0.34;
  ks.setJointPositions(jcm.joint_name, &jval);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // testing equality
  kinematic_constraints::JointConstraint jc2(kmodel);
  EXPECT_TRUE(jc2.configure(jcm));
  EXPECT_TRUE(jc2.enabled());
  EXPECT_TRUE(jc.equal(jc2, 1e-12));

  // if name not equal, not equal
  jcm.joint_name = "head_tilt_joint";
  EXPECT_TRUE(jc2.configure(jcm));
  EXPECT_FALSE(jc.equal(jc2, 1e-12));

  // if different, test margin behavior
  jcm.joint_name = "head_pan_joint";
  jcm.position = 0.3;
  EXPECT_TRUE(jc2.configure(jcm));
  EXPECT_FALSE(jc.equal(jc2, 1e-12));
  // exactly equal is still false
  EXPECT_FALSE(jc.equal(jc2, .1));
  EXPECT_TRUE(jc.equal(jc2, .101));

  // no name makes this false
  jcm.joint_name = "";
  jcm.position = 0.4;
  EXPECT_FALSE(jc2.configure(jcm));
  EXPECT_FALSE(jc2.enabled());
  EXPECT_FALSE(jc.equal(jc2, 1e-12));

  // no DOF makes this false
  jcm.joint_name = "base_footprint_joint";
  EXPECT_FALSE(jc2.configure(jcm));

  // clear means not enabled
  jcm.joint_name = "head_pan_joint";
  EXPECT_TRUE(jc2.configure(jcm));
  jc2.clear();
  EXPECT_FALSE(jc2.enabled());
  EXPECT_FALSE(jc.equal(jc2, 1e-12));
}

TEST_F(LoadPlanningModelsPr2, JointConstraintsCont)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  ks.update();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::JointConstraint jc(kmodel);
  moveit_msgs::JointConstraint jcm;

  jcm.joint_name = "l_wrist_roll_joint";
  jcm.position = 0.0;
  jcm.tolerance_above = 0.04;
  jcm.tolerance_below = 0.02;
  jcm.weight = 1.0;

  EXPECT_TRUE(jc.configure(jcm));

  std::map<std::string, double> jvals;

  // state should have zeros, and work
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // within the above tolerance
  jvals[jcm.joint_name] = .03;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // outside the above tolerance
  jvals[jcm.joint_name] = .05;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // inside the below tolerance
  jvals[jcm.joint_name] = -.01;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // ouside the below tolerance
  jvals[jcm.joint_name] = -.03;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // now testing wrap around from positive to negative
  jcm.position = 3.14;
  EXPECT_TRUE(jc.configure(jcm));

  // testing that wrap works
  jvals[jcm.joint_name] = 3.17;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
  EXPECT_TRUE(p1.satisfied);
  EXPECT_NEAR(p1.distance, 0.03, 1e-6);

  // testing that negative wrap works
  jvals[jcm.joint_name] = -3.14;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
  EXPECT_TRUE(p2.satisfied);
  EXPECT_NEAR(p2.distance, 0.003185, 1e-4);

  // over bound testing
  jvals[jcm.joint_name] = 3.19;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // reverses to other direction
  // but still tested using above tolerance
  jvals[jcm.joint_name] = -3.11;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // outside of the bound given the wrap
  jvals[jcm.joint_name] = -3.09;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // lower tolerance testing
  // within bounds
  jvals[jcm.joint_name] = 3.13;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // within outside
  jvals[jcm.joint_name] = 3.11;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // testing the other direction
  jcm.position = -3.14;
  EXPECT_TRUE(jc.configure(jcm));

  // should be governed by above tolerance
  jvals[jcm.joint_name] = -3.11;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // outside upper bound
  jvals[jcm.joint_name] = -3.09;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(jc.decide(ks).satisfied);

  // governed by lower bound
  jvals[jcm.joint_name] = 3.13;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // outside lower bound (but would be inside upper)
  jvals[jcm.joint_name] = 3.12;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // testing wrap
  jcm.position = 6.28;
  EXPECT_TRUE(jc.configure(jcm));

  // should wrap to zero
  jvals[jcm.joint_name] = 0.0;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(jc.decide(ks).satisfied);

  // should wrap to close and test to be near
  moveit_msgs::JointConstraint jcm2 = jcm;
  jcm2.position = -6.28;
  kinematic_constraints::JointConstraint jc2(kmodel);
  EXPECT_TRUE(jc.configure(jcm2));
  jc.equal(jc2, .02);
}

TEST_F(LoadPlanningModelsPr2, JointConstraintsMultiDOF)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();

  kinematic_constraints::JointConstraint jc(kmodel);
  moveit_msgs::JointConstraint jcm;
  jcm.joint_name = "world_joint";
  jcm.position = 3.14;
  jcm.tolerance_above = 0.1;
  jcm.tolerance_below = 0.05;
  jcm.weight = 1.0;

  // shouldn't work for multi-dof without local name
  EXPECT_FALSE(jc.configure(jcm));

  // this should, and function like any other single joint constraint
  jcm.joint_name = "world_joint/x";
  EXPECT_TRUE(jc.configure(jcm));

  std::map<std::string, double> jvals;
  jvals[jcm.joint_name] = 3.2;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p1 = jc.decide(ks);
  EXPECT_TRUE(p1.satisfied);

  jvals[jcm.joint_name] = 3.25;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p2 = jc.decide(ks);
  EXPECT_FALSE(p2.satisfied);

  jvals[jcm.joint_name] = -3.14;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p3 = jc.decide(ks);
  EXPECT_FALSE(p3.satisfied);

  // theta is continuous
  jcm.joint_name = "world_joint/theta";
  EXPECT_TRUE(jc.configure(jcm));

  jvals[jcm.joint_name] = -3.14;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p4 = jc.decide(ks);
  EXPECT_TRUE(p4.satisfied);

  jvals[jcm.joint_name] = 3.25;
  ks.setVariablePositions(jvals);
  kinematic_constraints::ConstraintEvaluationResult p5 = jc.decide(ks);
  EXPECT_FALSE(p5.satisfied);
}

TEST_F(LoadPlanningModelsPr2, PositionConstraintsFixed)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  ks.update(true);
  robot_state::Transforms tf(kmodel->getModelFrame());
  kinematic_constraints::PositionConstraint pc(kmodel);
  moveit_msgs::PositionConstraint pcm;

  // empty certainly means false
  EXPECT_FALSE(pc.configure(pcm, tf));

  pcm.link_name = "l_wrist_roll_link";
  pcm.target_point_offset.x = 0;
  pcm.target_point_offset.y = 0;
  pcm.target_point_offset.z = 0;
  pcm.constraint_region.primitives.resize(1);
  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;

  // no dimensions, so no valid regions
  EXPECT_FALSE(pc.configure(pcm, tf));

  pcm.constraint_region.primitives[0].dimensions.resize(1);
  pcm.constraint_region.primitives[0].dimensions[0] = 0.2;

  // no pose, so no valid region
  EXPECT_FALSE(pc.configure(pcm, tf));

  pcm.constraint_region.primitive_poses.resize(1);
  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
  pcm.weight = 1.0;

  // intentionally leaving header frame blank to test behavior
  EXPECT_FALSE(pc.configure(pcm, tf));

  pcm.header.frame_id = kmodel->getModelFrame();
  EXPECT_TRUE(pc.configure(pcm, tf));
  EXPECT_FALSE(pc.mobileReferenceFrame());

  EXPECT_TRUE(pc.decide(ks).satisfied);

  std::map<std::string, double> jvals;
  jvals["torso_lift_joint"] = 0.4;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_FALSE(pc.decide(ks).satisfied);
  EXPECT_TRUE(pc.equal(pc, 1e-12));

  // arbitrary offset that puts it back into the pose range
  pcm.target_point_offset.x = 0;
  pcm.target_point_offset.y = 0;
  pcm.target_point_offset.z = .15;

  EXPECT_TRUE(pc.configure(pcm, tf));
  EXPECT_TRUE(pc.hasLinkOffset());
  EXPECT_TRUE(pc.decide(ks).satisfied);

  pc.clear();
  EXPECT_FALSE(pc.enabled());

  // invalid quaternion results in zero quaternion
  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.w = 0.0;

  EXPECT_TRUE(pc.configure(pcm, tf));
  EXPECT_TRUE(pc.decide(ks).satisfied);
}

TEST_F(LoadPlanningModelsPr2, PositionConstraintsMobile)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  robot_state::Transforms tf(kmodel->getModelFrame());
  ks.update();

  kinematic_constraints::PositionConstraint pc(kmodel);
  moveit_msgs::PositionConstraint pcm;

  pcm.link_name = "l_wrist_roll_link";
  pcm.target_point_offset.x = 0;
  pcm.target_point_offset.y = 0;
  pcm.target_point_offset.z = 0;

  pcm.constraint_region.primitives.resize(1);
  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
  pcm.constraint_region.primitives[0].dimensions.resize(1);
  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.38 * 2.0;

  pcm.header.frame_id = "r_wrist_roll_link";

  pcm.constraint_region.primitive_poses.resize(1);
  pcm.constraint_region.primitive_poses[0].position.x = 0.0;
  pcm.constraint_region.primitive_poses[0].position.y = 0.6;
  pcm.constraint_region.primitive_poses[0].position.z = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
  pcm.weight = 1.0;

  EXPECT_FALSE(tf.isFixedFrame(pcm.link_name));
  EXPECT_TRUE(pc.configure(pcm, tf));
  EXPECT_TRUE(pc.mobileReferenceFrame());

  EXPECT_TRUE(pc.decide(ks).satisfied);

  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
  pcm.constraint_region.primitives[0].dimensions.resize(3);
  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1;
  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.1;
  EXPECT_TRUE(pc.configure(pcm, tf));

  std::map<std::string, double> jvals;
  jvals["l_shoulder_pan_joint"] = 0.4;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_TRUE(pc.decide(ks).satisfied);
  EXPECT_TRUE(pc.equal(pc, 1e-12));

  jvals["l_shoulder_pan_joint"] = -0.4;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_FALSE(pc.decide(ks).satisfied);

  // adding a second constrained region makes this work
  pcm.constraint_region.primitive_poses.resize(2);
  pcm.constraint_region.primitive_poses[1].position.x = 0.0;
  pcm.constraint_region.primitive_poses[1].position.y = 0.1;
  pcm.constraint_region.primitive_poses[1].position.z = 0.0;
  pcm.constraint_region.primitive_poses[1].orientation.x = 0.0;
  pcm.constraint_region.primitive_poses[1].orientation.y = 0.0;
  pcm.constraint_region.primitive_poses[1].orientation.z = 0.0;
  pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;

  pcm.constraint_region.primitives.resize(2);
  pcm.constraint_region.primitives[1].type = shape_msgs::SolidPrimitive::BOX;
  pcm.constraint_region.primitives[1].dimensions.resize(3);
  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.1;
  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.1;
  EXPECT_TRUE(pc.configure(pcm, tf));
  EXPECT_TRUE(pc.decide(ks, false).satisfied);
}

TEST_F(LoadPlanningModelsPr2, PositionConstraintsEquality)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::PositionConstraint pc(kmodel);
  kinematic_constraints::PositionConstraint pc2(kmodel);
  moveit_msgs::PositionConstraint pcm;

  pcm.link_name = "l_wrist_roll_link";
  pcm.target_point_offset.x = 0;
  pcm.target_point_offset.y = 0;
  pcm.target_point_offset.z = 0;

  pcm.constraint_region.primitives.resize(2);
  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
  pcm.constraint_region.primitives[0].dimensions.resize(1);
  pcm.constraint_region.primitives[0].dimensions[0] = 0.38 * 2.0;
  pcm.constraint_region.primitives[1].type = shape_msgs::SolidPrimitive::BOX;
  pcm.constraint_region.primitives[1].dimensions.resize(3);
  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 2.0;
  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 2.0;
  pcm.constraint_region.primitives[1].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 2.0;

  pcm.header.frame_id = "r_wrist_roll_link";
  pcm.constraint_region.primitive_poses.resize(2);
  pcm.constraint_region.primitive_poses[0].position.x = 0.0;
  pcm.constraint_region.primitive_poses[0].position.y = 0.6;
  pcm.constraint_region.primitive_poses[0].position.z = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
  pcm.constraint_region.primitive_poses[1].position.x = 2.0;
  pcm.constraint_region.primitive_poses[1].position.y = 0.0;
  pcm.constraint_region.primitive_poses[1].position.z = 0.0;
  pcm.constraint_region.primitive_poses[1].orientation.w = 1.0;
  pcm.weight = 1.0;

  EXPECT_TRUE(pc.configure(pcm, tf));
  EXPECT_TRUE(pc2.configure(pcm, tf));

  EXPECT_TRUE(pc.equal(pc2, .001));
  EXPECT_TRUE(pc2.equal(pc, .001));

  // putting regions in different order
  moveit_msgs::PositionConstraint pcm2 = pcm;
  pcm2.constraint_region.primitives[0] = pcm.constraint_region.primitives[1];
  pcm2.constraint_region.primitives[1] = pcm.constraint_region.primitives[0];

  pcm2.constraint_region.primitive_poses[0] = pcm.constraint_region.primitive_poses[1];
  pcm2.constraint_region.primitive_poses[1] = pcm.constraint_region.primitive_poses[0];

  EXPECT_TRUE(pc2.configure(pcm2, tf));
  EXPECT_TRUE(pc.equal(pc2, .001));
  EXPECT_TRUE(pc2.equal(pc, .001));

  // messing with one value breaks it
  pcm2.constraint_region.primitive_poses[0].position.z = .01;
  EXPECT_TRUE(pc2.configure(pcm2, tf));
  EXPECT_FALSE(pc.equal(pc2, .001));
  EXPECT_FALSE(pc2.equal(pc, .001));
  EXPECT_TRUE(pc.equal(pc2, .1));
  EXPECT_TRUE(pc2.equal(pc, .1));

  // adding an identical third shape to the last one is ok
  pcm2.constraint_region.primitive_poses[0].position.z = 0.0;
  pcm2.constraint_region.primitives.resize(3);
  pcm2.constraint_region.primitive_poses.resize(3);
  pcm2.constraint_region.primitives[2] = pcm2.constraint_region.primitives[0];
  pcm2.constraint_region.primitive_poses[2] = pcm2.constraint_region.primitive_poses[0];
  EXPECT_TRUE(pc2.configure(pcm2, tf));
  EXPECT_TRUE(pc.equal(pc2, .001));
  EXPECT_TRUE(pc2.equal(pc, .001));

  // but if we change it, it's not
  pcm2.constraint_region.primitives[2].dimensions[0] = 3.0;
  EXPECT_TRUE(pc2.configure(pcm2, tf));
  EXPECT_FALSE(pc.equal(pc2, .001));
  EXPECT_FALSE(pc2.equal(pc, .001));

  // changing the shape also changes it
  pcm2.constraint_region.primitives[2].dimensions[0] = pcm2.constraint_region.primitives[0].dimensions[0];
  pcm2.constraint_region.primitives[2].type = shape_msgs::SolidPrimitive::SPHERE;
  EXPECT_TRUE(pc2.configure(pcm2, tf));
  EXPECT_FALSE(pc.equal(pc2, .001));
}

TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  ks.update();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::OrientationConstraint oc(kmodel);

  moveit_msgs::OrientationConstraint ocm;

  EXPECT_FALSE(oc.configure(ocm, tf));

  ocm.link_name = "r_wrist_roll_link";

  // all we currently have to specify is the link name to get a valid constraint
  EXPECT_TRUE(oc.configure(ocm, tf));

  ocm.header.frame_id = kmodel->getModelFrame();
  ocm.orientation.x = 0.0;
  ocm.orientation.y = 0.0;
  ocm.orientation.z = 0.0;
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

  EXPECT_TRUE(oc.configure(ocm, tf));
  EXPECT_FALSE(oc.mobileReferenceFrame());

  EXPECT_FALSE(oc.decide(ks).satisfied);

  ocm.header.frame_id = ocm.link_name;
  EXPECT_TRUE(oc.configure(ocm, tf));

  EXPECT_TRUE(oc.decide(ks).satisfied);
  EXPECT_TRUE(oc.equal(oc, 1e-12));
  EXPECT_TRUE(oc.mobileReferenceFrame());

  ASSERT_TRUE(oc.getLinkModel());

  geometry_msgs::Pose p = tf2::toMsg(ks.getGlobalLinkTransform(oc.getLinkModel()->getName()));

  ocm.orientation = p.orientation;
  ocm.header.frame_id = kmodel->getModelFrame();
  EXPECT_TRUE(oc.configure(ocm, tf));
  EXPECT_TRUE(oc.decide(ks).satisfied);

  std::map<std::string, double> jvals;
  jvals["r_wrist_roll_joint"] = .05;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_TRUE(oc.decide(ks).satisfied);

  jvals["r_wrist_roll_joint"] = .11;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_FALSE(oc.decide(ks).satisfied);
}

TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  ks.update();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::VisibilityConstraint vc(kmodel);
  moveit_msgs::VisibilityConstraint vcm;

  EXPECT_FALSE(vc.configure(vcm, tf));

  vcm.sensor_pose.header.frame_id = "base_footprint";
  vcm.sensor_pose.pose.position.z = -1.0;
  vcm.sensor_pose.pose.orientation.y = 1.0;

  vcm.target_pose.header.frame_id = "base_footprint";
  vcm.target_pose.pose.position.z = -2.0;
  vcm.target_pose.pose.orientation.y = 0.0;
  vcm.target_pose.pose.orientation.w = 1.0;

  vcm.target_radius = .2;
  vcm.cone_sides = 10;
  vcm.max_view_angle = 0.0;
  vcm.max_range_angle = 0.0;
  vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_Z;
  vcm.weight = 1.0;

  EXPECT_TRUE(vc.configure(vcm, tf));
  // sensor and target are perfectly lined up
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  vcm.max_view_angle = .1;

  // true, even with view angle
  EXPECT_TRUE(vc.configure(vcm, tf));
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  // very slight angle, so still ok
  vcm.target_pose.pose.orientation.y = 0.03;
  vcm.target_pose.pose.orientation.w = .9995;
  EXPECT_TRUE(vc.configure(vcm, tf));
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  // a little bit more puts it over
  vcm.target_pose.pose.orientation.y = 0.06;
  vcm.target_pose.pose.orientation.w = .9981;
  EXPECT_TRUE(vc.configure(vcm, tf));
  EXPECT_FALSE(vc.decide(ks, true).satisfied);
}

TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsPR2)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  ks.update();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::VisibilityConstraint vc(kmodel);
  moveit_msgs::VisibilityConstraint vcm;

  vcm.sensor_pose.header.frame_id = "narrow_stereo_optical_frame";
  vcm.sensor_pose.pose.position.z = 0.05;
  vcm.sensor_pose.pose.orientation.w = 1.0;

  vcm.target_pose.header.frame_id = "l_gripper_r_finger_tip_link";
  vcm.target_pose.pose.position.z = 0.03;
  vcm.target_pose.pose.orientation.w = 1.0;

  vcm.cone_sides = 10;
  vcm.max_view_angle = 0.0;
  vcm.max_range_angle = 0.0;
  vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_Z;
  vcm.weight = 1.0;

  // false because target radius is 0.0
  EXPECT_FALSE(vc.configure(vcm, tf));

  // this is all fine
  vcm.target_radius = .05;
  EXPECT_TRUE(vc.configure(vcm, tf));
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  // this moves into collision with the cone, and should register false
  std::map<std::string, double> state_values;
  state_values["l_shoulder_lift_joint"] = .5;
  state_values["r_shoulder_pan_joint"] = .5;
  state_values["r_elbow_flex_joint"] = -1.4;
  ks.setVariablePositions(state_values);
  ks.update();
  EXPECT_FALSE(vc.decide(ks, true).satisfied);

  // this moves far enough away that it's fine
  state_values["r_shoulder_pan_joint"] = .4;
  ks.setVariablePositions(state_values);
  ks.update();
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  // this is in collision with the arm, but now the cone, and should be fine
  state_values["l_shoulder_lift_joint"] = 0;
  state_values["r_shoulder_pan_joint"] = .5;
  state_values["r_elbow_flex_joint"] = -.6;
  ks.setVariablePositions(state_values);
  ks.update();
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  // this shouldn't matter
  vcm.sensor_view_direction = moveit_msgs::VisibilityConstraint::SENSOR_X;
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  ks.setToDefaultValues();
  ks.update();
  // just hits finger tip
  vcm.target_radius = .01;
  vcm.target_pose.pose.position.z = 0.00;
  vcm.target_pose.pose.position.x = 0.035;
  EXPECT_TRUE(vc.configure(vcm, tf));
  EXPECT_TRUE(vc.decide(ks, true).satisfied);

  // larger target means it also hits finger
  vcm.target_radius = .05;
  EXPECT_TRUE(vc.configure(vcm, tf));
  EXPECT_FALSE(vc.decide(ks, true).satisfied);
}

TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSet)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::KinematicConstraintSet kcs(kmodel);
  EXPECT_TRUE(kcs.empty());

  moveit_msgs::JointConstraint jcm;
  jcm.joint_name = "head_pan_joint";
  jcm.position = 0.4;
  jcm.tolerance_above = 0.1;
  jcm.tolerance_below = 0.05;
  jcm.weight = 1.0;

  // this is a valid constraint
  std::vector<moveit_msgs::JointConstraint> jcv;
  jcv.push_back(jcm);
  EXPECT_TRUE(kcs.add(jcv));

  // but it isn't satisfied in the default state
  EXPECT_FALSE(kcs.decide(ks).satisfied);

  // now it is
  std::map<std::string, double> jvals;
  jvals[jcm.joint_name] = 0.41;
  ks.setVariablePositions(jvals);
  ks.update();
  EXPECT_TRUE(kcs.decide(ks).satisfied);

  // adding another constraint for a different joint
  EXPECT_FALSE(kcs.empty());
  kcs.clear();
  EXPECT_TRUE(kcs.empty());
  jcv.push_back(jcm);
  jcv.back().joint_name = "head_tilt_joint";
  EXPECT_TRUE(kcs.add(jcv));

  // now this one isn't satisfied
  EXPECT_FALSE(kcs.decide(ks).satisfied);

  // now it is
  jvals[jcv.back().joint_name] = 0.41;
  ks.setVariablePositions(jvals);
  EXPECT_TRUE(kcs.decide(ks).satisfied);

  // changing one joint outside the bounds makes it unsatisfied
  jvals[jcv.back().joint_name] = 0.51;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(kcs.decide(ks).satisfied);

  // one invalid constraint makes the add return false
  kcs.clear();
  jcv.back().joint_name = "no_joint";
  EXPECT_FALSE(kcs.add(jcv));

  // but we can still evaluate it succesfully for the remaining constraint
  EXPECT_TRUE(kcs.decide(ks).satisfied);

  // violating the remaining good constraint changes this
  jvals["head_pan_joint"] = 0.51;
  ks.setVariablePositions(jvals);
  EXPECT_FALSE(kcs.decide(ks).satisfied);
}

TEST_F(LoadPlanningModelsPr2, TestKinematicConstraintSetEquality)
{
  robot_state::RobotState ks(kmodel);
  ks.setToDefaultValues();
  robot_state::Transforms tf(kmodel->getModelFrame());

  kinematic_constraints::KinematicConstraintSet kcs(kmodel);
  kinematic_constraints::KinematicConstraintSet kcs2(kmodel);

  moveit_msgs::JointConstraint jcm;
  jcm.joint_name = "head_pan_joint";
  jcm.position = 0.4;
  jcm.tolerance_above = 0.1;
  jcm.tolerance_below = 0.05;
  jcm.weight = 1.0;

  moveit_msgs::PositionConstraint pcm;
  pcm.link_name = "l_wrist_roll_link";
  pcm.target_point_offset.x = 0;
  pcm.target_point_offset.y = 0;
  pcm.target_point_offset.z = 0;
  pcm.constraint_region.primitives.resize(1);
  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
  pcm.constraint_region.primitives[0].dimensions.resize(1);
  pcm.constraint_region.primitives[0].dimensions[0] = 0.2;

  pcm.constraint_region.primitive_poses.resize(1);
  pcm.constraint_region.primitive_poses[0].position.x = 0.55;
  pcm.constraint_region.primitive_poses[0].position.y = 0.2;
  pcm.constraint_region.primitive_poses[0].position.z = 1.25;
  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
  pcm.weight = 1.0;

  pcm.header.frame_id = kmodel->getModelFrame();

  // this is a valid constraint
  std::vector<moveit_msgs::JointConstraint> jcv;
  jcv.push_back(jcm);
  EXPECT_TRUE(kcs.add(jcv));

  std::vector<moveit_msgs::PositionConstraint> pcv;
  pcv.push_back(pcm);
  EXPECT_TRUE(kcs.add(pcv, tf));

  // now adding in reverse order
  EXPECT_TRUE(kcs2.add(pcv, tf));
  EXPECT_TRUE(kcs2.add(jcv));

  // should be true
  EXPECT_TRUE(kcs.equal(kcs2, .001));
  EXPECT_TRUE(kcs2.equal(kcs, .001));

  // adding another copy of one of the constraints doesn't change anything
  jcv.push_back(jcm);
  EXPECT_TRUE(kcs2.add(jcv));

  EXPECT_TRUE(kcs.equal(kcs2, .001));
  EXPECT_TRUE(kcs2.equal(kcs, .001));

  jcm.joint_name = "head_pan_joint";
  jcm.position = 0.35;
  jcm.tolerance_above = 0.1;
  jcm.tolerance_below = 0.05;
  jcm.weight = 1.0;

  jcv.push_back(jcm);
  EXPECT_TRUE(kcs2.add(jcv));

  EXPECT_FALSE(kcs.equal(kcs2, .001));
  EXPECT_FALSE(kcs2.equal(kcs, .001));

  // but they are within this margin
  EXPECT_TRUE(kcs.equal(kcs2, .1));
  EXPECT_TRUE(kcs2.equal(kcs, .1));
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}