default_constraint_samplers.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 */
00036 
00037 #include <moveit/constraint_samplers/default_constraint_samplers.h>
00038 #include <set>
00039 #include <cassert>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <boost/bind.hpp>
00042 
00043 bool constraint_samplers::JointConstraintSampler::configure(const moveit_msgs::Constraints& constr)
00044 {
00045   // construct the constraints
00046   std::vector<kinematic_constraints::JointConstraint> jc;
00047   for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
00048   {
00049     kinematic_constraints::JointConstraint j(scene_->getRobotModel());
00050     if (j.configure(constr.joint_constraints[i]))
00051       jc.push_back(j);
00052   }
00053 
00054   return jc.empty() ? false : configure(jc);
00055 }
00056 
00057 bool constraint_samplers::JointConstraintSampler::configure(
00058     const std::vector<kinematic_constraints::JointConstraint>& jc)
00059 {
00060   clear();
00061 
00062   if (!jmg_)
00063   {
00064     logError("NULL group specified for constraint sampler");
00065     return false;
00066   }
00067 
00068   // find and keep the constraints that operate on the group we sample
00069   // also keep bounds for joints as convenient
00070   std::map<std::string, JointInfo> bound_data;
00071   bool some_valid_constraint = false;
00072   for (std::size_t i = 0; i < jc.size(); ++i)
00073   {
00074     if (!jc[i].enabled())
00075       continue;
00076 
00077     const robot_model::JointModel* jm = jc[i].getJointModel();
00078     if (!jmg_->hasJointModel(jm->getName()))
00079       continue;
00080 
00081     some_valid_constraint = true;
00082 
00083     const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(jc[i].getJointVariableName());
00084     JointInfo ji;
00085     std::map<std::string, JointInfo>::iterator it = bound_data.find(jc[i].getJointVariableName());
00086     if (it != bound_data.end())
00087       ji = it->second;
00088     else
00089       ji.index_ = jmg_->getVariableGroupIndex(jc[i].getJointVariableName());
00090     ji.potentiallyAdjustMinMaxBounds(
00091         std::max(joint_bounds.min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()),
00092         std::min(joint_bounds.max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()));
00093 
00094     logDebug("Bounds for %s JointConstraint are %g %g", jc[i].getJointVariableName().c_str(), ji.min_bound_,
00095              ji.max_bound_);
00096 
00097     if (ji.min_bound_ > ji.max_bound_ + std::numeric_limits<double>::epsilon())
00098     {
00099       std::stringstream cs;
00100       jc[i].print(cs);
00101       logError("The constraints for joint '%s' are such that there are no possible values for the joint: min_bound: "
00102                "%g, max_bound: %g. Failing.\n",
00103                jm->getName().c_str(), ji.min_bound_, ji.max_bound_);
00104       clear();
00105       return false;
00106     }
00107     bound_data[jc[i].getJointVariableName()] = ji;
00108   }
00109 
00110   if (!some_valid_constraint)
00111   {
00112     logWarn("No valid joint constraints");
00113     return false;
00114   }
00115 
00116   for (std::map<std::string, JointInfo>::iterator it = bound_data.begin(); it != bound_data.end(); ++it)
00117     bounds_.push_back(it->second);
00118 
00119   // get a separate list of joints that are not bounded; we will sample these randomly
00120   const std::vector<const robot_model::JointModel*>& joints = jmg_->getJointModels();
00121   for (std::size_t i = 0; i < joints.size(); ++i)
00122     if (bound_data.find(joints[i]->getName()) == bound_data.end() && joints[i]->getVariableCount() > 0 &&
00123         joints[i]->getMimic() == NULL)
00124     {
00125       // check if all the vars of the joint are found in bound_data instead
00126       const std::vector<std::string>& vars = joints[i]->getVariableNames();
00127       if (vars.size() > 1)
00128       {
00129         bool all_found = true;
00130         for (std::size_t j = 0; j < vars.size(); ++j)
00131           if (bound_data.find(vars[j]) == bound_data.end())
00132           {
00133             all_found = false;
00134             break;
00135           }
00136         if (all_found)
00137           continue;
00138       }
00139       unbounded_.push_back(joints[i]);
00140       // Get the first variable name of this joint and find its index position in the planning group
00141       uindex_.push_back(jmg_->getVariableGroupIndex(vars[0]));
00142     }
00143   values_.resize(jmg_->getVariableCount());
00144   is_valid_ = true;
00145   return true;
00146 }
00147 
00148 bool constraint_samplers::JointConstraintSampler::sample(robot_state::RobotState& state,
00149                                                          const robot_state::RobotState& /* reference_state */,
00150                                                          unsigned int /* max_attempts */)
00151 {
00152   if (!is_valid_)
00153   {
00154     logWarn("JointConstraintSampler not configured, won't sample");
00155     return false;
00156   }
00157 
00158   // sample the unbounded joints first (in case some joint variables are bounded)
00159   std::vector<double> v;
00160   for (std::size_t i = 0; i < unbounded_.size(); ++i)
00161   {
00162     v.resize(unbounded_[i]->getVariableCount());
00163     unbounded_[i]->getVariableRandomPositions(random_number_generator_, &v[0]);
00164     for (std::size_t j = 0; j < v.size(); ++j)
00165       values_[uindex_[i] + j] = v[j];
00166   }
00167 
00168   // enforce the constraints for the constrained components (could be all of them)
00169   for (std::size_t i = 0; i < bounds_.size(); ++i)
00170     values_[bounds_[i].index_] = random_number_generator_.uniformReal(bounds_[i].min_bound_, bounds_[i].max_bound_);
00171 
00172   state.setJointGroupPositions(jmg_, values_);
00173 
00174   // we are always successful
00175   return true;
00176 }
00177 
00178 bool constraint_samplers::JointConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
00179 {
00180   return sample(state, state, max_attempts);
00181 }
00182 
00183 void constraint_samplers::JointConstraintSampler::clear()
00184 {
00185   ConstraintSampler::clear();
00186   bounds_.clear();
00187   unbounded_.clear();
00188   uindex_.clear();
00189   values_.clear();
00190 }
00191 
00192 constraint_samplers::IKSamplingPose::IKSamplingPose()
00193 {
00194 }
00195 
00196 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraint& pc)
00197   : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
00198 {
00199 }
00200 
00201 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc)
00202   : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
00203 {
00204 }
00205 
00206 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraint& pc,
00207                                                     const kinematic_constraints::OrientationConstraint& oc)
00208   : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
00209   , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
00210 {
00211 }
00212 
00213 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc)
00214   : position_constraint_(pc)
00215 {
00216 }
00217 
00218 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc)
00219   : orientation_constraint_(oc)
00220 {
00221 }
00222 
00223 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
00224                                                     const kinematic_constraints::OrientationConstraintPtr& oc)
00225   : position_constraint_(pc), orientation_constraint_(oc)
00226 {
00227 }
00228 
00229 void constraint_samplers::IKConstraintSampler::clear()
00230 {
00231   ConstraintSampler::clear();
00232   kb_.reset();
00233   ik_frame_ = "";
00234   transform_ik_ = false;
00235 }
00236 
00237 bool constraint_samplers::IKConstraintSampler::configure(const IKSamplingPose& sp)
00238 {
00239   clear();
00240   if (!sp.position_constraint_ && !sp.orientation_constraint_)
00241     return false;
00242   if ((!sp.orientation_constraint_ && !sp.position_constraint_->enabled()) ||
00243       (!sp.position_constraint_ && !sp.orientation_constraint_->enabled()) ||
00244       (sp.position_constraint_ && sp.orientation_constraint_ && !sp.position_constraint_->enabled() &&
00245        !sp.orientation_constraint_->enabled()))
00246   {
00247     logWarn("No enabled constraints in sampling pose");
00248     return false;
00249   }
00250 
00251   sampling_pose_ = sp;
00252   ik_timeout_ = jmg_->getDefaultIKTimeout();
00253   if (sampling_pose_.position_constraint_ && sampling_pose_.orientation_constraint_)
00254     if (sampling_pose_.position_constraint_->getLinkModel()->getName() !=
00255         sampling_pose_.orientation_constraint_->getLinkModel()->getName())
00256     {
00257       logError("Position and orientation constraints need to be specified for the same link in order to use IK-based "
00258                "sampling");
00259       return false;
00260     }
00261 
00262   if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->mobileReferenceFrame())
00263     frame_depends_.push_back(sampling_pose_.position_constraint_->getReferenceFrame());
00264   if (sampling_pose_.orientation_constraint_ && sampling_pose_.orientation_constraint_->mobileReferenceFrame())
00265     frame_depends_.push_back(sampling_pose_.orientation_constraint_->getReferenceFrame());
00266   kb_ = jmg_->getSolverInstance();
00267   if (!kb_)
00268   {
00269     logWarn("No solver instance in setup");
00270     is_valid_ = false;
00271     return false;
00272   }
00273   is_valid_ = loadIKSolver();
00274   return is_valid_;
00275 }
00276 
00277 bool constraint_samplers::IKConstraintSampler::configure(const moveit_msgs::Constraints& constr)
00278 {
00279   for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00280     for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00281       if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
00282       {
00283         kinematic_constraints::PositionConstraintPtr pc(
00284             new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
00285         kinematic_constraints::OrientationConstraintPtr oc(
00286             new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
00287         if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
00288             oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
00289           return configure(IKSamplingPose(pc, oc));
00290       }
00291 
00292   for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00293   {
00294     kinematic_constraints::PositionConstraintPtr pc(
00295         new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
00296     if (pc->configure(constr.position_constraints[p], scene_->getTransforms()))
00297       return configure(IKSamplingPose(pc));
00298   }
00299 
00300   for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00301   {
00302     kinematic_constraints::OrientationConstraintPtr oc(
00303         new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
00304     if (oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
00305       return configure(IKSamplingPose(oc));
00306   }
00307   return false;
00308 }
00309 
00310 double constraint_samplers::IKConstraintSampler::getSamplingVolume() const
00311 {
00312   double v = 1.0;
00313   if (sampling_pose_.position_constraint_)
00314   {
00315     const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
00316     double vol = 0;
00317     for (std::size_t i = 0; i < b.size(); ++i)
00318       vol += b[i]->computeVolume();
00319     if (!b.empty())
00320       v *= vol;
00321   }
00322 
00323   if (sampling_pose_.orientation_constraint_)
00324     v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
00325          sampling_pose_.orientation_constraint_->getYAxisTolerance() *
00326          sampling_pose_.orientation_constraint_->getZAxisTolerance();
00327   return v;
00328 }
00329 
00330 const std::string& constraint_samplers::IKConstraintSampler::getLinkName() const
00331 {
00332   if (sampling_pose_.orientation_constraint_)
00333     return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
00334   return sampling_pose_.position_constraint_->getLinkModel()->getName();
00335 }
00336 
00337 bool constraint_samplers::IKConstraintSampler::loadIKSolver()
00338 {
00339   if (!kb_)
00340   {
00341     logError("No IK solver");
00342     return false;
00343   }
00344 
00345   // check if we need to transform the request into the coordinate frame expected by IK
00346   ik_frame_ = kb_->getBaseFrame();
00347   transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame());
00348   if (!ik_frame_.empty() && ik_frame_[0] == '/')
00349     ik_frame_.erase(ik_frame_.begin());
00350   if (transform_ik_)
00351     if (!jmg_->getParentModel().hasLinkModel(ik_frame_))
00352     {
00353       logError("The IK solver expects requests in frame '%s' but this frame is not known to the sampler. Ignoring "
00354                "transformation (IK may fail)",
00355                ik_frame_.c_str());
00356       transform_ik_ = false;
00357     }
00358 
00359   // check if IK is performed for the desired link
00360   bool wrong_link = false;
00361   if (sampling_pose_.position_constraint_)
00362   {
00363     const moveit::core::LinkModel* lm = sampling_pose_.position_constraint_->getLinkModel();
00364     if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
00365     {
00366       wrong_link = true;
00367       const moveit::core::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
00368       for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
00369         if (moveit::core::Transforms::sameFrame(it->first->getName(), kb_->getTipFrame()))
00370         {
00371           sampling_pose_.position_constraint_->swapLinkModel(jmg_->getParentModel().getLinkModel(it->first->getName()),
00372                                                              it->second);
00373           wrong_link = false;
00374           break;
00375         }
00376     }
00377   }
00378 
00379   if (!wrong_link && sampling_pose_.orientation_constraint_)
00380   {
00381     const moveit::core::LinkModel* lm = sampling_pose_.orientation_constraint_->getLinkModel();
00382     if (!robot_state::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
00383     {
00384       wrong_link = true;
00385       const moveit::core::LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
00386       for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
00387         if (moveit::core::Transforms::sameFrame(it->first->getName(), kb_->getTipFrame()))
00388         {
00389           sampling_pose_.orientation_constraint_->swapLinkModel(
00390               jmg_->getParentModel().getLinkModel(it->first->getName()), it->second.rotation());
00391           wrong_link = false;
00392           break;
00393         }
00394     }
00395   }
00396 
00397   if (wrong_link)
00398   {
00399     logError("IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
00400              sampling_pose_.position_constraint_ ?
00401                  sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
00402                  sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
00403              kb_->getTipFrame().c_str());
00404     return false;
00405   }
00406   return true;
00407 }
00408 
00409 bool constraint_samplers::IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat,
00410                                                           const robot_state::RobotState& ks, unsigned int max_attempts)
00411 {
00412   if (ks.dirtyLinkTransforms())
00413   {
00414     // samplePose below requires accurate transforms
00415     logError("IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.");
00416     return false;
00417   }
00418 
00419   if (sampling_pose_.position_constraint_)
00420   {
00421     const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
00422     if (!b.empty())
00423     {
00424       bool found = false;
00425       std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
00426       for (std::size_t i = 0; i < b.size(); ++i)
00427         if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
00428         {
00429           found = true;
00430           break;
00431         }
00432       if (!found)
00433       {
00434         logError("Unable to sample a point inside the constraint region");
00435         return false;
00436       }
00437     }
00438     else
00439     {
00440       logError("Unable to sample a point inside the constraint region. Constraint region is empty when it should not "
00441                "be.");
00442       return false;
00443     }
00444 
00445     // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
00446     // model
00447     if (sampling_pose_.position_constraint_->mobileReferenceFrame())
00448       pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
00449   }
00450   else
00451   {
00452     // do FK for rand state
00453     robot_state::RobotState tempState(ks);
00454     tempState.setToRandomPositions(jmg_);
00455     pos = tempState.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
00456   }
00457 
00458   if (sampling_pose_.orientation_constraint_)
00459   {
00460     // sample a rotation matrix within the allowed bounds
00461     double angle_x =
00462         2.0 * (random_number_generator_.uniform01() - 0.5) *
00463         (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
00464     double angle_y =
00465         2.0 * (random_number_generator_.uniform01() - 0.5) *
00466         (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
00467     double angle_z =
00468         2.0 * (random_number_generator_.uniform01() - 0.5) *
00469         (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
00470     Eigen::Affine3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
00471                          Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
00472                          Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
00473     Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
00474     quat = Eigen::Quaterniond(reqr.rotation());
00475 
00476     // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
00477     // model
00478     if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
00479     {
00480       const Eigen::Affine3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
00481       Eigen::Affine3d rt(t.rotation() * quat.toRotationMatrix());
00482       quat = Eigen::Quaterniond(rt.rotation());
00483     }
00484   }
00485   else
00486   {
00487     // sample a random orientation
00488     double q[4];
00489     random_number_generator_.quaternion(q);
00490     quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
00491   }
00492 
00493   // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
00494   if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
00495     // the rotation matrix that corresponds to the desired orientation
00496     pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
00497 
00498   // we now have the transform we wish to perform IK for, in the planning frame
00499 
00500   if (transform_ik_)
00501   {
00502     // we need to convert this transform to the frame expected by the IK solver
00503     // both the planning frame and the frame for the IK are assumed to be robot links
00504     Eigen::Affine3d ikq(Eigen::Translation3d(pos) * quat.toRotationMatrix());
00505     ikq = ks.getFrameTransform(ik_frame_).inverse() * ikq;
00506     pos = ikq.translation();
00507     quat = Eigen::Quaterniond(ikq.rotation());
00508   }
00509 
00510   return true;
00511 }
00512 
00513 namespace constraint_samplers
00514 {
00515 namespace
00516 {
00517 void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
00518                                  const robot_state::GroupStateValidityCallbackFn& constraint,
00519                                  const geometry_msgs::Pose&, const std::vector<double>& ik_sol,
00520                                  moveit_msgs::MoveItErrorCodes& error_code)
00521 {
00522   const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
00523   std::vector<double> solution(bij.size());
00524   for (std::size_t i = 0; i < bij.size(); ++i)
00525     solution[i] = ik_sol[bij[i]];
00526   if (constraint(state, jmg, &solution[0]))
00527     error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00528   else
00529     error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00530 }
00531 }
00532 }
00533 
00534 bool constraint_samplers::IKConstraintSampler::sample(robot_state::RobotState& state,
00535                                                       const robot_state::RobotState& reference_state,
00536                                                       unsigned int max_attempts)
00537 {
00538   return sampleHelper(state, reference_state, max_attempts, false);
00539 }
00540 
00541 bool constraint_samplers::IKConstraintSampler::sampleHelper(robot_state::RobotState& state,
00542                                                             const robot_state::RobotState& reference_state,
00543                                                             unsigned int max_attempts, bool project)
00544 {
00545   if (!is_valid_)
00546   {
00547     logWarn("IKConstraintSampler not configured, won't sample");
00548     return false;
00549   }
00550 
00551   kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback;
00552   if (group_state_validity_callback_)
00553     adapted_ik_validity_callback =
00554         boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3);
00555 
00556   for (unsigned int a = 0; a < max_attempts; ++a)
00557   {
00558     // sample a point in the constraint region
00559     Eigen::Vector3d point;
00560     Eigen::Quaterniond quat;
00561     if (!samplePose(point, quat, reference_state, max_attempts))
00562     {
00563       if (verbose_)
00564         logInform("IK constraint sampler was unable to produce a pose to run IK for");
00565       return false;
00566     }
00567 
00568     geometry_msgs::Pose ik_query;
00569     ik_query.position.x = point.x();
00570     ik_query.position.y = point.y();
00571     ik_query.position.z = point.z();
00572     ik_query.orientation.x = quat.x();
00573     ik_query.orientation.y = quat.y();
00574     ik_query.orientation.z = quat.z();
00575     ik_query.orientation.w = quat.w();
00576 
00577     if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0))
00578       return true;
00579   }
00580   return false;
00581 }
00582 
00583 bool constraint_samplers::IKConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
00584 {
00585   return sampleHelper(state, state, max_attempts, true);
00586 }
00587 
00588 bool constraint_samplers::IKConstraintSampler::validate(robot_state::RobotState& state) const
00589 {
00590   state.update();
00591   return (!sampling_pose_.orientation_constraint_ ||
00592           sampling_pose_.orientation_constraint_->decide(state, verbose_).satisfied) &&
00593          (!sampling_pose_.position_constraint_ ||
00594           sampling_pose_.position_constraint_->decide(state, verbose_).satisfied);
00595 }
00596 
00597 bool constraint_samplers::IKConstraintSampler::callIK(
00598     const geometry_msgs::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback,
00599     double timeout, robot_state::RobotState& state, bool use_as_seed)
00600 {
00601   const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
00602   std::vector<double> seed(ik_joint_bijection.size(), 0.0);
00603   std::vector<double> vals;
00604 
00605   if (use_as_seed)
00606     state.copyJointGroupPositions(jmg_, vals);
00607   else
00608     // sample a seed value
00609     jmg_->getVariableRandomPositions(random_number_generator_, vals);
00610 
00611   assert(vals.size() == ik_joint_bijection.size());
00612   for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
00613     seed[i] = vals[ik_joint_bijection[i]];
00614 
00615   std::vector<double> ik_sol;
00616   moveit_msgs::MoveItErrorCodes error;
00617 
00618   if (adapted_ik_validity_callback ?
00619           kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
00620           kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
00621   {
00622     assert(ik_sol.size() == ik_joint_bijection.size());
00623     std::vector<double> solution(ik_joint_bijection.size());
00624     for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
00625       solution[ik_joint_bijection[i]] = ik_sol[i];
00626     state.setJointGroupPositions(jmg_, solution);
00627 
00628     return validate(state);
00629   }
00630   else
00631   {
00632     if (error.val != moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION &&
00633         error.val != moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE &&
00634         error.val != moveit_msgs::MoveItErrorCodes::TIMED_OUT)
00635       logError("IK solver failed with error %d", error.val);
00636     else if (verbose_)
00637       logInform("IK failed");
00638   }
00639   return false;
00640 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 14 2018 03:31:40