Planning Constraints from MoveIt Request

constrained_ik/include/constrained_ik/constrained_ik.h

@@ -101,6 +101,8 @@ class Constrained_IK
    */
   virtual void addConstraintsFromParamServer(const std::string &parameter_name);
 
+  virtual std::vector<std::pair<bool, Constraint*> > loadConstraintsFromParamServer(const std::string& parameter_name);
+
   /**
    * @brief computes the inverse kinematics for the given pose of the tip link
    * @param goal cartesian pose to solve the inverse kinematics about

constrained_ik/include/constrained_ik/constraint.h

@@ -50,6 +50,15 @@ class Constraint
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+  static const unsigned TYPE_POSITION = 1u;
+  static const unsigned TYPE_ORIENTATION = 2u;
+  static const unsigned TYPE_OTHER = 4u;
+
+  virtual unsigned constraintType() const
+  {
+    return TYPE_OTHER;
+  }
+
   /**
    * @brief This structure is to be used by all constraints to store specific data
    * that needs to get updated every iteration of the solver.

constrained_ik/include/constrained_ik/constraints/goal_orientation.h

@@ -56,6 +56,11 @@ class GoalOrientation : public Constraint
 
   GoalOrientation();
 
+  virtual unsigned constraintType() const
+  {
+    return TYPE_ORIENTATION;
+  }
+
   /** @brief see base class for documentation*/
   constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override;
 

constrained_ik/include/constrained_ik/constraints/goal_pose.h

@@ -50,6 +50,12 @@ class GoalPose : public ConstraintGroup
     this->add(position_);
     this->add(orientation_);
   }
+
+  virtual unsigned constraintType() const
+  {
+    return position_->constraintType() | orientation_->constraintType();
+  }
+
   /**
    * @brief Setter for the orientation weights
    * @param weight_orientation values to set the orientation weights

constrained_ik/include/constrained_ik/constraints/goal_position.h

@@ -57,6 +57,11 @@ class GoalPosition : public Constraint
 
   GoalPosition();
 
+  virtual unsigned constraintType() const
+  {
+    return TYPE_POSITION;
+  }
+
   /** @brief see base class for documentation*/
   constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override;
 

constrained_ik/include/constrained_ik/constraints/goal_tool_pointing.h

@@ -57,6 +57,11 @@ class GoalToolPointing : public Constraint
 
   GoalToolPointing();
 
+  virtual unsigned constraintType() const
+  {
+    return TYPE_POSITION | TYPE_ORIENTATION;
+  }
+
   /** @brief see base class for documentation*/
   constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override;
 

constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h

@@ -125,6 +125,7 @@ namespace constrained_ik
     /** @brief Reset the planners IK solver configuration to it default settings */
     void resetSolverConfiguration();
 
+
   private:
     /**
      * @brief Preform position and orientation interpolation between start and stop.
@@ -137,6 +138,15 @@ namespace constrained_ik
     std::vector<Eigen::Affine3d,Eigen::aligned_allocator<Eigen::Affine3d> >
     interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, double ds, double dt) const;
 
+    // Constraint additions
+    std::vector<std::pair<bool, Constraint*> > resolveConstraints() const;
+
+    Constraint* createConstraint(const moveit_msgs::PositionConstraint& pos_constraint) const;
+
+    Constraint* createConstraint(const moveit_msgs::OrientationConstraint& orient_constraint) const;
+
+    std::vector<std::pair<bool, Constraint*> > default_constraints_;    /**< These parameters are loaded from YAML files and may be overridden by
+                                                                             constraints specified through planning requests. */
     double translational_discretization_step_;    /**< Max translational discretization step */
     double orientational_discretization_step_;    /**< Max orientational discretization step */
     bool debug_mode_;                             /**< Debug state */

constrained_ik/src/constrained_ik.cpp

@@ -45,27 +45,43 @@ Constrained_IK::Constrained_IK(const ros::NodeHandle &nh) : nh_(nh)
 }
 
 void Constrained_IK::addConstraintsFromParamServer(const std::string &parameter_name)
+{
+  std::vector<std::pair<bool, Constraint*> > constraints = loadConstraintsFromParamServer(parameter_name);
+
+  for (std::size_t i = 0; i < constraints.size(); ++i)
+  {
+    const bool is_primary = constraints[i].first;
+    Constraint* constraint = constraints[i].second;
+
+    if (is_primary)
+      addConstraint(constraint, constraint_types::Primary);
+    else
+      addConstraint(constraint, constraint_types::Auxiliary);
+  }
+}
+
+std::vector<std::pair<bool, Constraint*> > Constrained_IK::loadConstraintsFromParamServer(const std::string& parameter_name)
 {
   XmlRpc::XmlRpcValue constraints_xml;
   boost::shared_ptr<pluginlib::ClassLoader<constrained_ik::Constraint> > constraint_loader;
 
   constraint_loader.reset(new pluginlib::ClassLoader<constrained_ik::Constraint>("constrained_ik", "constrained_ik::Constraint"));
 
+  std::vector<std::pair<bool, Constraint*> > constraints;
+
   if (!nh_.getParam(parameter_name, constraints_xml))
   {
-    ROS_ERROR("Unable to find ros parameter: %s", parameter_name.c_str());
-    ROS_BREAK();
-    return;
+    ROS_WARN("Unable to find ros parameter: %s", parameter_name.c_str());
+    return constraints;
   }
 
   if(constraints_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
   {
     ROS_ERROR("ROS parameter %s must be an array", parameter_name.c_str());
-    ROS_BREAK();
-    return;
+    return constraints;
   }
 
-  for (int i=0; i<constraints_xml.size(); ++i)
+  for (int i = 0; i < constraints_xml.size(); ++i)
   {
     XmlRpc::XmlRpcValue constraint_xml = constraints_xml[i];
 
@@ -81,25 +97,21 @@ void Constrained_IK::addConstraintsFromParamServer(const std::string &parameter_
       try
       {
         constraint = constraint_loader->createUnmanagedInstance(class_name);
-
         constraint->loadParameters(constraint_xml);
-        if (is_primary)
-          addConstraint(constraint, constraint_types::Primary);
-        else
-          addConstraint(constraint, constraint_types::Auxiliary);
-
+        constraints.push_back(std::make_pair(is_primary, constraint));
       }
       catch (pluginlib::PluginlibException& ex)
       {
         ROS_ERROR("Couldn't load constraint named %s.\n Error: %s", class_name.c_str(), ex.what());
-        ROS_BREAK();
       }
     }
     else
     {
       ROS_ERROR("Constraint must have class(string) and primary(boolean) members");
     }
   }
+
+  return constraints;
 }
 
 void Constrained_IK::loadDefaultSolverConfiguration()