default_constraint_samplers.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 #include <set>
39 #include <cassert>
41 #include <boost/bind.hpp>
42 
43 bool constraint_samplers::JointConstraintSampler::configure(const moveit_msgs::Constraints& constr)
44 {
45  // construct the constraints
46  std::vector<kinematic_constraints::JointConstraint> jc;
47  for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
48  {
50  if (j.configure(constr.joint_constraints[i]))
51  jc.push_back(j);
52  }
53 
54  return jc.empty() ? false : configure(jc);
55 }
56 
58  const std::vector<kinematic_constraints::JointConstraint>& jc)
59 {
60  clear();
61 
62  if (!jmg_)
63  {
64  CONSOLE_BRIDGE_logError("NULL group specified for constraint sampler");
65  return false;
66  }
67 
68  // find and keep the constraints that operate on the group we sample
69  // also keep bounds for joints as convenient
70  std::map<std::string, JointInfo> bound_data;
71  bool some_valid_constraint = false;
72  for (std::size_t i = 0; i < jc.size(); ++i)
73  {
74  if (!jc[i].enabled())
75  continue;
76 
77  const robot_model::JointModel* jm = jc[i].getJointModel();
78  if (!jmg_->hasJointModel(jm->getName()))
79  continue;
80 
81  some_valid_constraint = true;
82 
83  const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(jc[i].getJointVariableName());
84  JointInfo ji;
85  std::map<std::string, JointInfo>::iterator it = bound_data.find(jc[i].getJointVariableName());
86  if (it != bound_data.end())
87  ji = it->second;
88  else
89  ji.index_ = jmg_->getVariableGroupIndex(jc[i].getJointVariableName());
91  std::max(joint_bounds.min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()),
92  std::min(joint_bounds.max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()));
93 
94  CONSOLE_BRIDGE_logDebug("Bounds for %s JointConstraint are %g %g", jc[i].getJointVariableName().c_str(),
95  ji.min_bound_, ji.max_bound_);
96 
97  if (ji.min_bound_ > ji.max_bound_ + std::numeric_limits<double>::epsilon())
98  {
99  std::stringstream cs;
100  jc[i].print(cs);
101  CONSOLE_BRIDGE_logError("The constraints for joint '%s' are such that there are no possible values for the "
102  "joint: min_bound: %g, max_bound: %g. Failing.\n",
103  jm->getName().c_str(), ji.min_bound_, ji.max_bound_);
104  clear();
105  return false;
106  }
107  bound_data[jc[i].getJointVariableName()] = ji;
108  }
109 
110  if (!some_valid_constraint)
111  {
112  CONSOLE_BRIDGE_logWarn("No valid joint constraints");
113  return false;
114  }
115 
116  for (std::map<std::string, JointInfo>::iterator it = bound_data.begin(); it != bound_data.end(); ++it)
117  bounds_.push_back(it->second);
118 
119  // get a separate list of joints that are not bounded; we will sample these randomly
120  const std::vector<const robot_model::JointModel*>& joints = jmg_->getJointModels();
121  for (std::size_t i = 0; i < joints.size(); ++i)
122  if (bound_data.find(joints[i]->getName()) == bound_data.end() && joints[i]->getVariableCount() > 0 &&
123  joints[i]->getMimic() == NULL)
124  {
125  // check if all the vars of the joint are found in bound_data instead
126  const std::vector<std::string>& vars = joints[i]->getVariableNames();
127  if (vars.size() > 1)
128  {
129  bool all_found = true;
130  for (std::size_t j = 0; j < vars.size(); ++j)
131  if (bound_data.find(vars[j]) == bound_data.end())
132  {
133  all_found = false;
134  break;
135  }
136  if (all_found)
137  continue;
138  }
139  unbounded_.push_back(joints[i]);
140  // Get the first variable name of this joint and find its index position in the planning group
141  uindex_.push_back(jmg_->getVariableGroupIndex(vars[0]));
142  }
143  values_.resize(jmg_->getVariableCount());
144  is_valid_ = true;
145  return true;
146 }
147 
149  const robot_state::RobotState& /* reference_state */,
150  unsigned int /* max_attempts */)
151 {
152  if (!is_valid_)
153  {
154  CONSOLE_BRIDGE_logWarn("JointConstraintSampler not configured, won't sample");
155  return false;
156  }
157 
158  // sample the unbounded joints first (in case some joint variables are bounded)
159  std::vector<double> v;
160  for (std::size_t i = 0; i < unbounded_.size(); ++i)
161  {
162  v.resize(unbounded_[i]->getVariableCount());
163  unbounded_[i]->getVariableRandomPositions(random_number_generator_, &v[0]);
164  for (std::size_t j = 0; j < v.size(); ++j)
165  values_[uindex_[i] + j] = v[j];
166  }
167 
168  // enforce the constraints for the constrained components (could be all of them)
169  for (std::size_t i = 0; i < bounds_.size(); ++i)
170  values_[bounds_[i].index_] = random_number_generator_.uniformReal(bounds_[i].min_bound_, bounds_[i].max_bound_);
171 
173 
174  // we are always successful
175  return true;
176 }
177 
179 {
180  return sample(state, state, max_attempts);
181 }
182 
184 {
186  bounds_.clear();
187  unbounded_.clear();
188  uindex_.clear();
189  values_.clear();
190 }
191 
193 {
194 }
195 
197  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
198 {
199 }
200 
202  : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
203 {
204 }
205 
208  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
209  , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
210 {
211 }
212 
213 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc)
215 {
216 }
217 
218 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc)
220 {
221 }
222 
223 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
224  const kinematic_constraints::OrientationConstraintPtr& oc)
226 {
227 }
228 
230 {
232  kb_.reset();
233  ik_frame_ = "";
234  transform_ik_ = false;
235  eef_to_ik_tip_transform_ = Eigen::Affine3d::Identity();
236  need_eef_to_ik_tip_transform_ = false;
237 }
238 
240 {
241  clear();
243  return false;
244  if ((!sp.orientation_constraint_ && !sp.position_constraint_->enabled()) ||
245  (!sp.position_constraint_ && !sp.orientation_constraint_->enabled()) ||
247  !sp.orientation_constraint_->enabled()))
248  {
249  CONSOLE_BRIDGE_logWarn("No enabled constraints in sampling pose");
250  return false;
251  }
252 
253  sampling_pose_ = sp;
254  ik_timeout_ = jmg_->getDefaultIKTimeout();
255  if (sampling_pose_.position_constraint_ && sampling_pose_.orientation_constraint_)
256  if (sampling_pose_.position_constraint_->getLinkModel()->getName() !=
257  sampling_pose_.orientation_constraint_->getLinkModel()->getName())
258  {
259  CONSOLE_BRIDGE_logError("Position and orientation constraints need to be specified for the same link in "
260  "order to use IK-based sampling");
261  return false;
262  }
263 
264  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->mobileReferenceFrame())
265  frame_depends_.push_back(sampling_pose_.position_constraint_->getReferenceFrame());
266  if (sampling_pose_.orientation_constraint_ && sampling_pose_.orientation_constraint_->mobileReferenceFrame())
267  frame_depends_.push_back(sampling_pose_.orientation_constraint_->getReferenceFrame());
268  kb_ = jmg_->getSolverInstance();
269  if (!kb_)
270  {
271  CONSOLE_BRIDGE_logWarn("No solver instance in setup");
272  is_valid_ = false;
273  return false;
274  }
275  is_valid_ = loadIKSolver();
276  return is_valid_;
277 }
278 
279 bool constraint_samplers::IKConstraintSampler::configure(const moveit_msgs::Constraints& constr)
280 {
281  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
282  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
283  if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
284  {
285  kinematic_constraints::PositionConstraintPtr pc(
286  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
287  kinematic_constraints::OrientationConstraintPtr oc(
288  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
289  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
290  oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
291  return configure(IKSamplingPose(pc, oc));
292  }
293 
294  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
295  {
296  kinematic_constraints::PositionConstraintPtr pc(
297  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
298  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()))
299  return configure(IKSamplingPose(pc));
300  }
301 
302  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
303  {
304  kinematic_constraints::OrientationConstraintPtr oc(
305  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
306  if (oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
307  return configure(IKSamplingPose(oc));
308  }
309  return false;
310 }
311 
313 {
314  double v = 1.0;
315  if (sampling_pose_.position_constraint_)
316  {
317  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
318  double vol = 0;
319  for (std::size_t i = 0; i < b.size(); ++i)
320  vol += b[i]->computeVolume();
321  if (!b.empty())
322  v *= vol;
323  }
324 
325  if (sampling_pose_.orientation_constraint_)
326  v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
327  sampling_pose_.orientation_constraint_->getYAxisTolerance() *
328  sampling_pose_.orientation_constraint_->getZAxisTolerance();
329  return v;
330 }
331 
333 {
334  if (sampling_pose_.orientation_constraint_)
335  return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
336  return sampling_pose_.position_constraint_->getLinkModel()->getName();
337 }
338 
340 {
341  if (!kb_)
342  {
343  CONSOLE_BRIDGE_logError("No IK solver");
344  return false;
345  }
346 
347  // check if we need to transform the request into the coordinate frame expected by IK
348  ik_frame_ = kb_->getBaseFrame();
349  transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame());
350  if (!ik_frame_.empty() && ik_frame_[0] == '/')
351  ik_frame_.erase(ik_frame_.begin());
352  if (transform_ik_)
353  if (!jmg_->getParentModel().hasLinkModel(ik_frame_))
354  {
355  CONSOLE_BRIDGE_logError("The IK solver expects requests in frame '%s' but this frame is not known to the "
356  "sampler. Ignoring transformation (IK may fail)",
357  ik_frame_.c_str());
358  transform_ik_ = false;
359  }
360 
361  // check if IK is performed for the desired link
362  bool wrong_link = false;
363  if (sampling_pose_.position_constraint_)
364  {
365  const moveit::core::LinkModel* lm = sampling_pose_.position_constraint_->getLinkModel();
366  if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
367  {
368  wrong_link = true;
370  for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
371  if (moveit::core::Transforms::sameFrame(it->first->getName(), kb_->getTipFrame()))
372  {
373  eef_to_ik_tip_transform_ = it->second;
374  need_eef_to_ik_tip_transform_ = true;
375  wrong_link = false;
376  break;
377  }
378  }
379  }
380 
381  if (!wrong_link && sampling_pose_.orientation_constraint_)
382  {
383  const moveit::core::LinkModel* lm = sampling_pose_.orientation_constraint_->getLinkModel();
384  if (!robot_state::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
385  {
386  wrong_link = true;
388  for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
389  if (moveit::core::Transforms::sameFrame(it->first->getName(), kb_->getTipFrame()))
390  {
391  eef_to_ik_tip_transform_ = it->second;
392  need_eef_to_ik_tip_transform_ = true;
393  wrong_link = false;
394  break;
395  }
396  }
397  }
398 
399  if (wrong_link)
400  {
401  CONSOLE_BRIDGE_logError("IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
402  sampling_pose_.position_constraint_ ?
403  sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
404  sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
405  kb_->getTipFrame().c_str());
406  return false;
407  }
408  return true;
409 }
410 
411 bool constraint_samplers::IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat,
412  const robot_state::RobotState& ks, unsigned int max_attempts)
413 {
414  if (ks.dirtyLinkTransforms())
415  {
416  // samplePose below requires accurate transforms
417  CONSOLE_BRIDGE_logError("IKConstraintSampler received dirty robot state, but valid transforms are required. "
418  "Failing.");
419  return false;
420  }
421 
422  if (sampling_pose_.position_constraint_)
423  {
424  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
425  if (!b.empty())
426  {
427  bool found = false;
428  std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
429  for (std::size_t i = 0; i < b.size(); ++i)
430  if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
431  {
432  found = true;
433  break;
434  }
435  if (!found)
436  {
437  CONSOLE_BRIDGE_logError("Unable to sample a point inside the constraint region");
438  return false;
439  }
440  }
441  else
442  {
443  CONSOLE_BRIDGE_logError("Unable to sample a point inside the constraint region. "
444  "Constraint region is empty when it should not be.");
445  return false;
446  }
447 
448  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
449  // model
450  if (sampling_pose_.position_constraint_->mobileReferenceFrame())
451  pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
452  }
453  else
454  {
455  // do FK for rand state
456  robot_state::RobotState tempState(ks);
457  tempState.setToRandomPositions(jmg_);
458  pos = tempState.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
459  }
460 
461  if (sampling_pose_.orientation_constraint_)
462  {
463  // sample a rotation matrix within the allowed bounds
464  double angle_x =
465  2.0 * (random_number_generator_.uniform01() - 0.5) *
466  (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
467  double angle_y =
468  2.0 * (random_number_generator_.uniform01() - 0.5) *
469  (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
470  double angle_z =
471  2.0 * (random_number_generator_.uniform01() - 0.5) *
472  (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
473  Eigen::Affine3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
474  Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
475  Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
476  Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
477  quat = Eigen::Quaterniond(reqr.rotation());
478 
479  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
480  // model
481  if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
482  {
483  const Eigen::Affine3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
484  Eigen::Affine3d rt(t.rotation() * quat.toRotationMatrix());
485  quat = Eigen::Quaterniond(rt.rotation());
486  }
487  }
488  else
489  {
490  // sample a random orientation
491  double q[4];
492  random_number_generator_.quaternion(q);
493  quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
494  }
495 
496  // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
497  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
498  // the rotation matrix that corresponds to the desired orientation
499  pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
500 
501  return true;
502 }
503 
504 namespace constraint_samplers
505 {
506 namespace
507 {
508 void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
510  const geometry_msgs::Pose&, const std::vector<double>& ik_sol,
511  moveit_msgs::MoveItErrorCodes& error_code)
512 {
513  const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
514  std::vector<double> solution(bij.size());
515  for (std::size_t i = 0; i < bij.size(); ++i)
516  solution[i] = ik_sol[bij[i]];
517  if (constraint(state, jmg, &solution[0]))
518  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
519  else
521 }
522 }
523 }
524 
526  const robot_state::RobotState& reference_state,
527  unsigned int max_attempts)
528 {
529  return sampleHelper(state, reference_state, max_attempts, false);
530 }
531 
533  const robot_state::RobotState& reference_state,
534  unsigned int max_attempts, bool project)
535 {
536  if (!is_valid_)
537  {
538  CONSOLE_BRIDGE_logWarn("IKConstraintSampler not configured, won't sample");
539  return false;
540  }
541 
542  kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback;
543  if (group_state_validity_callback_)
544  adapted_ik_validity_callback =
545  boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3);
546 
547  for (unsigned int a = 0; a < max_attempts; ++a)
548  {
549  // sample a point in the constraint region
550  Eigen::Vector3d point;
551  Eigen::Quaterniond quat;
552  if (!samplePose(point, quat, reference_state, max_attempts))
553  {
554  if (verbose_)
555  CONSOLE_BRIDGE_logInform("IK constraint sampler was unable to produce a pose to run IK for");
556  return false;
557  }
558 
559  // we now have the transform we wish to perform IK for, in the planning frame
560  if (transform_ik_)
561  {
562  // we need to convert this transform to the frame expected by the IK solver
563  // both the planning frame and the frame for the IK are assumed to be robot links
564  Eigen::Affine3d ikq(Eigen::Translation3d(point) * quat.toRotationMatrix());
565  ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq;
566  point = ikq.translation();
567  quat = Eigen::Quaterniond(ikq.rotation());
568  }
569 
570  if (need_eef_to_ik_tip_transform_)
571  {
572  // After sampling the pose needs to be transformed to the ik chain tip
573  Eigen::Affine3d ikq(Eigen::Translation3d(point) * quat.toRotationMatrix());
574  ikq = ikq * eef_to_ik_tip_transform_;
575  point = ikq.translation();
576  quat = Eigen::Quaterniond(ikq.rotation());
577  }
578 
579  geometry_msgs::Pose ik_query;
580  ik_query.position.x = point.x();
581  ik_query.position.y = point.y();
582  ik_query.position.z = point.z();
583  ik_query.orientation.x = quat.x();
584  ik_query.orientation.y = quat.y();
585  ik_query.orientation.z = quat.z();
586  ik_query.orientation.w = quat.w();
587 
588  if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0))
589  return true;
590  }
591  return false;
592 }
593 
595 {
596  return sampleHelper(state, state, max_attempts, true);
597 }
598 
600 {
601  state.update();
602  return (!sampling_pose_.orientation_constraint_ ||
603  sampling_pose_.orientation_constraint_->decide(state, verbose_).satisfied) &&
604  (!sampling_pose_.position_constraint_ ||
605  sampling_pose_.position_constraint_->decide(state, verbose_).satisfied);
606 }
607 
609  const geometry_msgs::Pose& ik_query, const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback,
610  double timeout, robot_state::RobotState& state, bool use_as_seed)
611 {
612  const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
613  std::vector<double> seed(ik_joint_bijection.size(), 0.0);
614  std::vector<double> vals;
615 
616  if (use_as_seed)
617  state.copyJointGroupPositions(jmg_, vals);
618  else
619  // sample a seed value
620  jmg_->getVariableRandomPositions(random_number_generator_, vals);
621 
622  assert(vals.size() == ik_joint_bijection.size());
623  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
624  seed[i] = vals[ik_joint_bijection[i]];
625 
626  std::vector<double> ik_sol;
627  moveit_msgs::MoveItErrorCodes error;
628 
629  if (adapted_ik_validity_callback ?
630  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
631  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
632  {
633  assert(ik_sol.size() == ik_joint_bijection.size());
634  std::vector<double> solution(ik_joint_bijection.size());
635  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
636  solution[ik_joint_bijection[i]] = ik_sol[i];
637  state.setJointGroupPositions(jmg_, solution);
638 
639  return validate(state);
640  }
641  else
642  {
644  error.val != moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE &&
646  CONSOLE_BRIDGE_logError("IK solver failed with error %d", error.val);
647  else if (verbose_)
648  CONSOLE_BRIDGE_logInform("IK failed");
649  }
650  return false;
651 }
#define CONSOLE_BRIDGE_logWarn(fmt,...)
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
Class for constraints on the orientation of a link.
bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, robot_state::RobotState &state, bool use_as_seed)
Actually calls IK on the given pose, generating a random seed state.
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1354
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures the IK constraint given a constraints message.
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
bool validate(robot_state::RobotState &state) const
Class for handling single DOF joint constraints.
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
virtual void clear()
Clears all data from the constraint.
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:59
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces an IK sample.
#define CONSOLE_BRIDGE_logDebug(fmt,...)
const robot_model::JointModelGroup * jmg_
Holds the joint model group associated with this constraint.
bool loadIKSolver()
Performs checks and sets various internal values associated with the IK solver.
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
virtual void clear()
Clears all data from the constraint.
static const int NO_IK_SOLUTION
Representation and evaluation of kinematic constraints.
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:660
virtual void clear()
Clears all data from the constraint.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
bool sampleHelper(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts, bool project)
void update(bool force=false)
Update all transforms.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
std::vector< JointInfo > bounds_
The bounds for any joint with bounds that are more restrictive than the joint limits.
const std::string & getLinkName() const
Gets the link name associated with this sampler.
bool dirtyLinkTransforms() const
Definition: robot_state.h:1431
double uniformReal(double lower_bound, double upper_bound)
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
The signature for a callback that can compute IK.
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Class for constraints on the XYZ position of a link.
#define CONSOLE_BRIDGE_logError(fmt,...)
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &ks, unsigned int max_attempts)
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
An internal structure used for maintaining constraints on a particular joint.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:612
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool is_valid_
Holds the value for validity.
std::vector< const robot_model::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:110
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h:64
planning_scene::PlanningSceneConstPtr scene_
Holds the planning scene.
#define CONSOLE_BRIDGE_logInform(fmt,...)
bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const robot_state::RobotState &ks, unsigned int max_attempts)
Returns a pose that falls within the constraint regions.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Apr 21 2018 02:54:51