00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
00069
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
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
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
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& ,
00150 unsigned int )
00151 {
00152 if (!is_valid_)
00153 {
00154 logWarn("JointConstraintSampler not configured, won't sample");
00155 return false;
00156 }
00157
00158
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
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
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
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
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
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
00446
00447 if (sampling_pose_.position_constraint_->mobileReferenceFrame())
00448 pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
00449 }
00450 else
00451 {
00452
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
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
00477
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
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
00494 if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
00495
00496 pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
00497
00498
00499
00500 if (transform_ik_)
00501 {
00502
00503
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
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
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 }