fix #15

constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h

@@ -163,6 +163,22 @@ class ConstraintSampler
               const kinematic_state::KinematicState &reference_state)
   {
     return sample(jsg, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
+  } 
+
+  /** 
+   * \brief Project a sample given the constraints, updating the joint state
+   * group. The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in
+   * as the maximum number of attempts to make to project the sample.
+   *
+   * @param [out] jsg The joint state group which specifies the state to be projected, according to the constraints 
+   * @param [in] reference_state Reference state that will be used to do transforms or perform other actions
+   *
+   * @return True if a sample was successfully projected, false otherwise
+   */
+  bool project(kinematic_state::JointStateGroup *jsg, 
+               const kinematic_state::KinematicState &reference_state)
+  {
+    return project(jsg, reference_state, DEFAULT_MAX_SAMPLING_ATTEMPTS);
   }
 
   /** 
@@ -178,13 +194,27 @@ class ConstraintSampler
   virtual bool sample(kinematic_state::JointStateGroup *jsg, 
                       const kinematic_state::KinematicState &reference_state, 
                       unsigned int max_attempts) = 0;
-
+
+  /** 
+   * \brief Project a sample given the constraints, updating the joint state
+   * group. This function allows the parameter max_attempts to be set.
+   *
+   * @param [out] jsg The joint state group which specifies the state to be projected, according to the constraints 
+   * @param [in] reference_state Reference state that will be used to do transforms or perform other actions
+   * @param [in] max_attempts The maximum number of times to attempt to draw a sample.  If no sample has been drawn in this number of attempts, false will be returned.
+   *
+   * @return True if a sample was successfully projected, false otherwise
+   */
+  virtual bool project(kinematic_state::JointStateGroup *jsg, 
+                       const kinematic_state::KinematicState &reference_state, 
+                       unsigned int max_attempts) = 0;
+
   /** 
    * \brief Returns whether or not the constraint sampler is valid or not.  To be valid, the joint model group must be available in the kinematic model.  
    * 
    * @return True if the sampler is valid, and otherwise false.
    */
-  bool isValid() const
+  bool isValid(void) const
   {
     return is_valid_;
   }
@@ -195,7 +225,7 @@ class ConstraintSampler
    * \brief Clears all data from the constraint.
    * 
    */
-  virtual void clear();
+  virtual void clear(void);
 
   bool                                     is_valid_;  /**< \brief  Holds the value for validity */
 

constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h

@@ -117,19 +117,14 @@ class JointConstraintSampler : public ConstraintSampler
    */  
   bool configure(const std::vector<kinematic_constraints::JointConstraint> &jc);
 
-  /** 
-   * \brief Takes a sample given the constraints, populating joint state group.
-   * 
-   * @param [out] jsg A joint state group.  The name must match the group passed through the constructor.
-   * @param [in] ks A reference, state, unused in this function
-   * @param [in] max_attempts The max attempts to take a constraint, unused in this function as all attempt will be successful
-   * 
-   * @return True if the constraint is valid and the joint state group matches the configured group.  Otherwise false.
-   */
   virtual bool sample(kinematic_state::JointStateGroup *jsg, 
                       const kinematic_state::KinematicState &ks,  
                       unsigned int max_attempts);
-
+
+  virtual bool project(kinematic_state::JointStateGroup *jsg, 
+                       const kinematic_state::KinematicState &reference_state, 
+                       unsigned int max_attempts);
+
   /** 
    * \brief Gets the number of constrained joints - joints that have an
    * additional bound beyond the joint limits.
@@ -445,6 +440,9 @@ class IKConstraintSampler : public ConstraintSampler
    */
   virtual bool sample(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts);
 
+  virtual bool project(kinematic_state::JointStateGroup *jsg, 
+                       const kinematic_state::KinematicState &reference_state, 
+                       unsigned int max_attempts);
   /** 
    * \brief Returns a pose that falls within the constraint regions. 
    * 
@@ -486,10 +484,14 @@ class IKConstraintSampler : public ConstraintSampler
    * @param ik_query The pose for solving IK, assumed to be for the tip frame in the base frame
    * @param timeout The timeout for the IK search 
    * @param jsg The joint state group into which to place the solution
+   * @param use_as_seed If true, the state values in jsg are used as seed for the IK
    * 
    * @return True if IK returns successfully with the timeout, and otherwise false.
    */
-  bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, kinematic_state::JointStateGroup *jsg);
+  bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout,
+              kinematic_state::JointStateGroup *jsg, bool use_as_seed);
+
+  bool sampleHelper(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts, bool project);
 
   random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random generator used by the sampler */
   IKSamplingPose                        sampling_pose_; /**< \brief Holder for the pose used for sampling */

constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h

@@ -151,7 +151,9 @@ class UnionConstraintSampler : public ConstraintSampler
    * @return True if all invidual samplers return true
    */
   virtual bool sample(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts);
-
+
+  virtual bool project(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts);
+
 protected:
 
   std::vector<ConstraintSamplerPtr> samplers_; /**< \brief Holder for sorted internal list of samplers*/

constraint_samplers/src/default_constraint_samplers.cpp

@@ -132,13 +132,13 @@ bool constraint_samplers::JointConstraintSampler::sample(kinematic_state::JointS
                                                          const kinematic_state::KinematicState & /* ks */,
                                                          unsigned int /* max_attempts */)
 {
-  if(!is_valid_) 
+  if (!is_valid_) 
   {
     logWarn("JointConstraintSampler not configured, won't sample");
     return false;
   }
 
-  if(jsg->getName() != getGroupName()) 
+  if (jsg->getName() != getGroupName()) 
   {
     logWarn("JointConstraintSampler sample function called with name %s which is not the group %s for which it was configured",
             jsg->getName().c_str(), getGroupName().c_str());
@@ -163,8 +163,15 @@ bool constraint_samplers::JointConstraintSampler::sample(kinematic_state::JointS
   // we are always successful
   return true;
 }
-
-void constraint_samplers::JointConstraintSampler::clear()
+
+bool constraint_samplers::JointConstraintSampler::project(kinematic_state::JointStateGroup *jsg, 
+                                                          const kinematic_state::KinematicState &reference_state, 
+                                                          unsigned int max_attempts)
+{
+  return sample(jsg, reference_state, max_attempts);
+}
+
+void constraint_samplers::JointConstraintSampler::clear(void)
 {
   ConstraintSampler::clear();
   bounds_.clear();
@@ -460,11 +467,16 @@ void samplingIkCallbackFnAdapter(kinematic_state::JointStateGroup *jsg, const ki
 }
 
 bool constraint_samplers::IKConstraintSampler::sample(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts)
+{
+  return sampleHelper(jsg, ks, max_attempts, false);
+}
+
+bool constraint_samplers::IKConstraintSampler::sampleHelper(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts, bool project)
 {
   if (!is_valid_)
     return false;
 
-  if(jsg->getName() != getGroupName()) 
+  if (jsg->getName() != getGroupName()) 
   {
     logWarn("IKConstraintSampler sample function called with name %s which is not the group %s for which it was configured",
             jsg->getName().c_str(), getGroupName().c_str());
@@ -492,21 +504,33 @@ bool constraint_samplers::IKConstraintSampler::sample(kinematic_state::JointStat
     ik_query.orientation.z = quat.z();
     ik_query.orientation.w = quat.w();        
 
-    if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, jsg))
+    if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, jsg, project))
       return true; 
   }
   return false;
 }
 
-bool constraint_samplers::IKConstraintSampler::callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback,
-                                                      double timeout, kinematic_state::JointStateGroup *jsg) 
+bool constraint_samplers::IKConstraintSampler::project(kinematic_state::JointStateGroup *jsg, 
+                                                       const kinematic_state::KinematicState &reference_state, 
+                                                       unsigned int max_attempts)
 {
-  // sample a seed value
-  std::vector<double> vals;
-  jmg_->getVariableRandomValues(random_number_generator_, vals);
+  return sampleHelper(jsg, reference_state, max_attempts, true);
+}
+
+bool constraint_samplers::IKConstraintSampler::callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback,
+                                                      double timeout, kinematic_state::JointStateGroup *jsg, bool use_as_seed)
+{ 
   const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
+  std::vector<double> seed(ik_joint_bijection.size(), 0.0); 
+  std::vector<double> vals;
+
+  if (use_as_seed)
+    jsg->getVariableValues(vals);
+  else
+    // sample a seed value
+    jmg_->getVariableRandomValues(random_number_generator_, vals);
+
   assert(vals.size() == ik_joint_bijection.size());
-  std::vector<double> seed(ik_joint_bijection.size(), 0.0);
   for (std::size_t i = 0 ; i < ik_joint_bijection.size() ; ++i)
     seed[ik_joint_bijection[i]] = vals[i];
 

constraint_samplers/src/union_constraint_sampler.cpp

@@ -152,3 +152,32 @@ bool constraint_samplers::UnionConstraintSampler::sample(kinematic_state::JointS
 
   return true;
 }
+
+bool constraint_samplers::UnionConstraintSampler::project(kinematic_state::JointStateGroup *jsg, const kinematic_state::KinematicState &ks, unsigned int max_attempts)
+{
+  if (samplers_.size() >= 1)
+  {
+    if (!samplers_[0]->project(jsg->getKinematicState()->getJointStateGroup(samplers_[0]->getJointModelGroup()->getName()), ks, max_attempts))
+      return false;
+  }
+
+  if (samplers_.size() > 1)
+  {
+    kinematic_state::KinematicState temp = ks;
+    *(temp.getJointStateGroup(jsg->getName())) = *jsg;
+
+    for (std::size_t i = 1 ; i < samplers_.size() ; ++i)
+    {
+      kinematic_state::JointStateGroup *x = jsg->getKinematicState()->getJointStateGroup(samplers_[i]->getJointModelGroup()->getName());
+      if (samplers_[i]->project(x, temp, max_attempts))
+      {
+        if (i + 1 < samplers_.size())
+          *(temp.getJointStateGroup(samplers_[i]->getJointModelGroup()->getName())) = *x;
+      }
+      else
+        return false;
+    }
+  }
+
+  return true;
+}