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