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 <cassert>
39 #include <functional>
40 
41 namespace constraint_samplers
42 {
43 random_numbers::RandomNumberGenerator createSeededRNG(const std::string& seed_param)
44 {
45  int rng_seed;
46  if (ros::param::get(seed_param, rng_seed))
47  {
48  ROS_DEBUG_STREAM_NAMED("constraint_samplers", "Creating random number generator with seed " << rng_seed);
50  }
51  else
52  {
54  }
55 }
56 
57 bool JointConstraintSampler::configure(const moveit_msgs::Constraints& constr)
58 {
59  // construct the constraints
60  std::vector<kinematic_constraints::JointConstraint> jc;
61  for (const moveit_msgs::JointConstraint& joint_constraint : constr.joint_constraints)
62  {
64  if (j.configure(joint_constraint))
65  jc.push_back(j);
66  }
67 
68  return jc.empty() ? false : configure(jc);
69 }
70 
71 bool JointConstraintSampler::configure(const std::vector<kinematic_constraints::JointConstraint>& jc)
72 {
73  clear();
74 
75  if (!jmg_)
76  {
77  ROS_ERROR_NAMED("constraint_samplers", "NULL group specified for constraint sampler");
78  return false;
79  }
80 
81  // find and keep the constraints that operate on the group we sample
82  // also keep bounds for joints as convenient
83  std::map<std::string, JointInfo> bound_data;
84  bool some_valid_constraint = false;
85  for (const kinematic_constraints::JointConstraint& joint_constraint : jc)
86  {
87  if (!joint_constraint.enabled())
88  continue;
89 
90  const moveit::core::JointModel* jm = joint_constraint.getJointModel();
91  if (!jmg_->hasJointModel(jm->getName()))
92  continue;
93 
94  some_valid_constraint = true;
95 
96  const moveit::core::VariableBounds& joint_bounds = jm->getVariableBounds(joint_constraint.getJointVariableName());
97  JointInfo ji;
98  std::map<std::string, JointInfo>::iterator it = bound_data.find(joint_constraint.getJointVariableName());
99  if (it != bound_data.end())
100  ji = it->second;
101  else
102  ji.index_ = jmg_->getVariableGroupIndex(joint_constraint.getJointVariableName());
104  std::max(joint_bounds.min_position_,
105  joint_constraint.getDesiredJointPosition() - joint_constraint.getJointToleranceBelow()),
106  std::min(joint_bounds.max_position_,
107  joint_constraint.getDesiredJointPosition() + joint_constraint.getJointToleranceAbove()));
108 
109  ROS_DEBUG_NAMED("constraint_samplers", "Bounds for %s JointConstraint are %g %g",
110  joint_constraint.getJointVariableName().c_str(), ji.min_bound_, ji.max_bound_);
111 
112  if (ji.min_bound_ > ji.max_bound_ + std::numeric_limits<double>::epsilon())
113  {
114  std::stringstream cs;
115  joint_constraint.print(cs);
116  ROS_ERROR_NAMED("constraint_samplers",
117  "The constraints for joint '%s' are such that "
118  "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n",
119  jm->getName().c_str(), ji.min_bound_, ji.max_bound_);
120  clear();
121  return false;
122  }
123  bound_data[joint_constraint.getJointVariableName()] = ji;
124  }
125 
126  if (!some_valid_constraint)
127  {
128  ROS_WARN_NAMED("constraint_samplers", "No valid joint constraints");
129  return false;
130  }
131 
132  for (std::pair<const std::string, JointInfo>& it : bound_data)
133  bounds_.push_back(it.second);
134 
135  // get a separate list of joints that are not bounded; we will sample these randomly
136  const std::vector<const moveit::core::JointModel*>& joints = jmg_->getJointModels();
137  for (const moveit::core::JointModel* joint : joints)
138  if (bound_data.find(joint->getName()) == bound_data.end() && joint->getVariableCount() > 0 &&
139  joint->getMimic() == nullptr)
140  {
141  // check if all the vars of the joint are found in bound_data instead
142  const std::vector<std::string>& vars = joint->getVariableNames();
143  if (vars.size() > 1)
144  {
145  bool all_found = true;
146  for (const std::string& var : vars)
147  if (bound_data.find(var) == bound_data.end())
148  {
149  all_found = false;
150  break;
151  }
152  if (all_found)
153  continue;
154  }
155  unbounded_.push_back(joint);
156  // Get the first variable name of this joint and find its index position in the planning group
157  uindex_.push_back(jmg_->getVariableGroupIndex(vars[0]));
158  }
159  values_.resize(jmg_->getVariableCount());
160  is_valid_ = true;
161  return true;
162 }
163 
165  const moveit::core::RobotState& /* reference_state */,
166  unsigned int /* max_attempts */)
167 {
168  if (!is_valid_)
169  {
170  ROS_WARN_NAMED("constraint_samplers", "JointConstraintSampler not configured, won't sample");
171  return false;
172  }
173 
174  // sample the unbounded joints first (in case some joint variables are bounded)
175  std::vector<double> v;
176  for (std::size_t i = 0; i < unbounded_.size(); ++i)
177  {
178  v.resize(unbounded_[i]->getVariableCount());
179  unbounded_[i]->getVariableRandomPositions(random_number_generator_, &v[0]);
180  for (std::size_t j = 0; j < v.size(); ++j)
181  values_[uindex_[i] + j] = v[j];
182  }
183 
184  // enforce the constraints for the constrained components (could be all of them)
185  for (const JointInfo& bound : bounds_)
186  values_[bound.index_] = random_number_generator_.uniformReal(bound.min_bound_, bound.max_bound_);
187 
189 
190  // we are always successful
191  return true;
192 }
193 
194 bool JointConstraintSampler::project(moveit::core::RobotState& state, unsigned int max_attempts)
195 {
196  return sample(state, state, max_attempts);
197 }
198 
200 {
202  bounds_.clear();
203  unbounded_.clear();
204  uindex_.clear();
205  values_.clear();
206 }
207 
209 {
210 }
211 
213  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
214 {
215 }
216 
218  : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
219 {
220 }
221 
224  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
225  , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
226 {
227 }
228 
229 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc) : position_constraint_(pc)
230 {
231 }
232 
233 IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc) : orientation_constraint_(oc)
234 {
235 }
236 
237 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
238  const kinematic_constraints::OrientationConstraintPtr& oc)
239  : position_constraint_(pc), orientation_constraint_(oc)
240 {
241 }
242 
244 {
246  kb_.reset();
247  ik_frame_ = "";
248  transform_ik_ = false;
249  eef_to_ik_tip_transform_ = Eigen::Isometry3d::Identity();
251 }
252 
254 {
255  clear();
257  return false;
258  if ((!sp.orientation_constraint_ && !sp.position_constraint_->enabled()) ||
259  (!sp.position_constraint_ && !sp.orientation_constraint_->enabled()) ||
261  !sp.orientation_constraint_->enabled()))
262  {
263  ROS_WARN_NAMED("constraint_samplers", "No enabled constraints in sampling pose");
264  return false;
265  }
266 
267  sampling_pose_ = sp;
270  if (sampling_pose_.position_constraint_->getLinkModel()->getName() !=
271  sampling_pose_.orientation_constraint_->getLinkModel()->getName())
272  {
273  ROS_ERROR_NAMED("constraint_samplers",
274  "Position and orientation constraints need to be specified for the same link "
275  "in order to use IK-based sampling");
276  return false;
277  }
278 
280  frame_depends_.push_back(sampling_pose_.position_constraint_->getReferenceFrame());
282  frame_depends_.push_back(sampling_pose_.orientation_constraint_->getReferenceFrame());
284  if (!kb_)
285  {
286  ROS_WARN_NAMED("constraint_samplers", "No solver instance in setup");
287  is_valid_ = false;
288  return false;
289  }
291  return is_valid_;
292 }
293 
294 bool IKConstraintSampler::configure(const moveit_msgs::Constraints& constr)
295 {
296  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
297  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
298  if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
299  {
300  kinematic_constraints::PositionConstraintPtr pc(
301  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
302  kinematic_constraints::OrientationConstraintPtr oc(
304  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
305  oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
306  return configure(IKSamplingPose(pc, oc));
307  }
308 
309  for (const moveit_msgs::PositionConstraint& position_constraint : constr.position_constraints)
310  {
311  kinematic_constraints::PositionConstraintPtr pc(
312  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
313  if (pc->configure(position_constraint, scene_->getTransforms()))
314  return configure(IKSamplingPose(pc));
315  }
316 
317  for (const moveit_msgs::OrientationConstraint& orientation_constraint : constr.orientation_constraints)
318  {
319  kinematic_constraints::OrientationConstraintPtr oc(
321  if (oc->configure(orientation_constraint, scene_->getTransforms()))
322  return configure(IKSamplingPose(oc));
323  }
324  return false;
325 }
326 
328 {
329  double v = 1.0;
331  {
332  const std::vector<bodies::BodyPtr>& constraint_regions =
333  sampling_pose_.position_constraint_->getConstraintRegions();
334  double vol = 0;
335  for (const bodies::BodyPtr& constraint_region : constraint_regions)
336  vol += constraint_region->computeVolume();
337  if (!constraint_regions.empty())
338  v *= vol;
339  }
340 
342  v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
343  sampling_pose_.orientation_constraint_->getYAxisTolerance() *
344  sampling_pose_.orientation_constraint_->getZAxisTolerance();
345  return v;
346 }
347 
348 const std::string& IKConstraintSampler::getLinkName() const
349 {
351  return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
352  return sampling_pose_.position_constraint_->getLinkModel()->getName();
353 }
354 
356 {
357  if (!kb_)
358  {
359  ROS_ERROR_NAMED("constraint_samplers", "No IK solver");
360  return false;
361  }
362 
363  // check if we need to transform the request into the coordinate frame expected by IK
364  ik_frame_ = kb_->getBaseFrame();
366  if (!ik_frame_.empty() && ik_frame_[0] == '/')
367  ik_frame_.erase(ik_frame_.begin());
368  if (transform_ik_)
370  {
371  ROS_ERROR_NAMED("constraint_samplers",
372  "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. "
373  "Ignoring transformation (IK may fail)",
374  ik_frame_.c_str());
375  transform_ik_ = false;
376  }
377 
378  // check if IK is performed for the desired link
379  bool wrong_link = false;
381  {
382  const moveit::core::LinkModel* lm = sampling_pose_.position_constraint_->getLinkModel();
383  if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
384  {
385  wrong_link = true;
387  for (const std::pair<const moveit::core::LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
388  if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame()))
389  {
390  eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract
392  wrong_link = false;
393  break;
394  }
395  }
396  }
397 
398  if (!wrong_link && sampling_pose_.orientation_constraint_)
399  {
401  if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
402  {
403  wrong_link = true;
405  for (const std::pair<const moveit::core::LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
406  if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame()))
407  {
408  eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract
410  wrong_link = false;
411  break;
412  }
413  }
414  }
415 
416  if (wrong_link)
417  {
418  ROS_ERROR_NAMED("constraint_samplers",
419  "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
421  sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
422  sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
423  kb_->getTipFrame().c_str());
424  return false;
425  }
426  return true;
427 }
428 
429 bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const moveit::core::RobotState& ks,
430  unsigned int max_attempts)
431 {
432  if (ks.dirtyLinkTransforms())
433  {
434  // samplePose below requires accurate transforms
435  ROS_ERROR_NAMED("constraint_samplers",
436  "IKConstraintSampler received dirty robot state, but valid transforms are required. "
437  "Failing.");
438  return false;
439  }
440 
442  {
443  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
444  if (!b.empty())
445  {
446  bool found = false;
447  std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
448  for (std::size_t i = 0; i < b.size(); ++i)
449  if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
450  {
451  found = true;
452  break;
453  }
454  if (!found)
455  {
456  ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region");
457  return false;
458  }
459  }
460  else
461  {
462  ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region. "
463  "Constraint region is empty when it should not be.");
464  return false;
465  }
466 
467  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
468  // model
469  if (sampling_pose_.position_constraint_->mobileReferenceFrame())
470  pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
471  }
472  else
473  {
474  // do FK for rand state
475  moveit::core::RobotState temp_state(ks);
476  temp_state.setToRandomPositions(jmg_);
477  pos = temp_state.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
478  }
479 
481  {
482  // sample a rotation matrix within the allowed bounds
483  double angle_x =
484  2.0 * (random_number_generator_.uniform01() - 0.5) *
485  (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
486  double angle_y =
487  2.0 * (random_number_generator_.uniform01() - 0.5) *
488  (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
489  double angle_z =
490  2.0 * (random_number_generator_.uniform01() - 0.5) *
491  (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
492 
493  Eigen::Isometry3d diff;
494  if (sampling_pose_.orientation_constraint_->getParameterizationType() ==
495  moveit_msgs::OrientationConstraint::XYZ_EULER_ANGLES)
496  {
497  diff = Eigen::Isometry3d(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
498  Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
499  Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
500  }
501  else if (sampling_pose_.orientation_constraint_->getParameterizationType() ==
502  moveit_msgs::OrientationConstraint::ROTATION_VECTOR)
503  {
504  Eigen::Vector3d rotation_vector(angle_x, angle_y, angle_z);
505  diff = Eigen::Isometry3d(Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized()));
506  }
507  else
508  {
509  /* The parameterization type should be validated in configure, so this should never happen. */
510  ROS_ERROR_STREAM_NAMED("default_constraint_samplers",
511  "The parameterization type for the orientation constraints is invalid.");
512  }
513 
514  // diff is isometry by construction
515  // getDesiredRotationMatrix() returns a valid rotation matrix by contract
516  // reqr has thus to be a valid isometry
517  Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
518  quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized
519 
520  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
521  // model
522  if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
523  {
524  // getFrameTransform() returns a valid isometry by contract
525  const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
526  // rt is isometry by construction
527  Eigen::Isometry3d rt(t.linear() * quat);
528  quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized
529  }
530  }
531  else
532  {
533  // sample a random orientation
534  double q[4];
536  quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]); // quat is normalized by contract
537  }
538 
539  // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
541  // the rotation matrix that corresponds to the desired orientation
542  pos = pos - quat * sampling_pose_.position_constraint_->getLinkOffset();
543 
544  return true;
545 }
546 
547 namespace
548 {
549 void samplingIkCallbackFnAdapter(moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
551  const std::vector<double>& ik_sol, moveit_msgs::MoveItErrorCodes& error_code)
552 {
553  const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
554  std::vector<double> solution(bij.size());
555  for (std::size_t i = 0; i < bij.size(); ++i)
556  solution[i] = ik_sol[bij[i]];
557  if (constraint(state, jmg, &solution[0]))
558  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
559  else
561 }
562 } // namespace
563 
565  unsigned int max_attempts)
566 {
567  return sampleHelper(state, reference_state, max_attempts, false);
568 }
569 
571  unsigned int max_attempts, bool project)
572 {
573  if (!is_valid_)
574  {
575  ROS_WARN_NAMED("constraint_samplers", "IKConstraintSampler not configured, won't sample");
576  return false;
577  }
578 
579  kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback;
581  adapted_ik_validity_callback = [this, state_ptr = &state](const geometry_msgs::Pose& /*unused*/,
582  const std::vector<double>& joints,
583  moveit_msgs::MoveItErrorCodes& error_code) {
584  return samplingIkCallbackFnAdapter(state_ptr, jmg_, group_state_validity_callback_, joints, error_code);
585  };
586 
587  for (unsigned int a = 0; a < max_attempts; ++a)
588  {
589  // sample a point in the constraint region
591  Eigen::Quaterniond quat; // quat is normalized by contract
592  if (!samplePose(point, quat, reference_state, max_attempts))
593  {
594  if (verbose_)
595  ROS_INFO_NAMED("constraint_samplers", "IK constraint sampler was unable to produce a pose to run IK for");
596  return false;
597  }
598 
599  // we now have the transform we wish to perform IK for, in the planning frame
600  if (transform_ik_)
601  {
602  // we need to convert this transform to the frame expected by the IK solver
603  // both the planning frame and the frame for the IK are assumed to be robot links
604  Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction
605  // getFrameTransform() returns a valid isometry by contract
606  ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; // valid isometry * valid isometry
607  point = ikq.translation();
608  quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized
609  }
610 
612  {
613  // After sampling the pose needs to be transformed to the ik chain tip
614  Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction
615  ikq = ikq * eef_to_ik_tip_transform_; // eef_to_ik_tip_transform_ is valid isometry (checked in loadIKSolver())
616  point = ikq.translation();
617  quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized
618  }
619 
620  geometry_msgs::Pose ik_query;
621  ik_query.position.x = point.x();
622  ik_query.position.y = point.y();
623  ik_query.position.z = point.z();
624  ik_query.orientation.x = quat.x();
625  ik_query.orientation.y = quat.y();
626  ik_query.orientation.z = quat.z();
627  ik_query.orientation.w = quat.w();
628 
629  if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0))
630  return true;
631  }
632  return false;
633 }
634 
635 bool IKConstraintSampler::project(moveit::core::RobotState& state, unsigned int max_attempts)
636 {
637  return sampleHelper(state, state, max_attempts, true);
638 }
639 
641 {
642  state.update();
644  sampling_pose_.orientation_constraint_->decide(state, verbose_).satisfied) &&
646  sampling_pose_.position_constraint_->decide(state, verbose_).satisfied);
647 }
648 
649 bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query,
650  const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback,
651  double timeout, moveit::core::RobotState& state, bool use_as_seed)
652 {
653  const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
654  std::vector<double> seed(ik_joint_bijection.size(), 0.0);
655  std::vector<double> vals;
656 
657  if (use_as_seed)
658  state.copyJointGroupPositions(jmg_, vals);
659  else
660  // sample a seed value
662 
663  assert(vals.size() == ik_joint_bijection.size());
664  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
665  seed[i] = vals[ik_joint_bijection[i]];
666 
667  std::vector<double> ik_sol;
668  moveit_msgs::MoveItErrorCodes error;
669 
670  if (adapted_ik_validity_callback ?
671  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
672  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
673  {
674  assert(ik_sol.size() == ik_joint_bijection.size());
675  std::vector<double> solution(ik_joint_bijection.size());
676  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
677  solution[ik_joint_bijection[i]] = ik_sol[i];
678  state.setJointGroupPositions(jmg_, solution);
679 
680  return validate(state);
681  }
682  else
683  {
685  error.val != moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE &&
687  ROS_ERROR_NAMED("constraint_samplers", "IK solver failed with error %d", error.val);
688  else if (verbose_)
689  ROS_INFO_NAMED("constraint_samplers", "IK failed");
690  }
691  return false;
692 }
693 
694 } // end of namespace constraint_samplers
constraint_samplers::IKSamplingPose
A structure for potentially holding a position constraint and an orientation constraint for use durin...
Definition: default_constraint_samplers.h:245
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
constraint_samplers::ConstraintSampler::is_valid_
bool is_valid_
Holds the value for validity.
Definition: constraint_sampler.h:317
moveit::core::RobotModel::getModelFrame
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState)....
Definition: robot_model.h:160
moveit::core::GroupStateValidityCallbackFn
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:136
constraint_samplers::createSeededRNG
random_numbers::RandomNumberGenerator createSeededRNG(const std::string &seed_param)
Definition: default_constraint_samplers.cpp:75
random_numbers::RandomNumberGenerator::uniform01
double uniform01(void)
pr2_arm_kinematics::TIMED_OUT
static const int TIMED_OUT
Definition: pr2_arm_kinematics_plugin.h:96
constraint_samplers::IKConstraintSampler::getSamplingVolume
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
Definition: default_constraint_samplers.cpp:359
default_constraint_samplers.h
constraint_samplers::IKConstraintSampler::sampleHelper
bool sampleHelper(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts, bool project)
Definition: default_constraint_samplers.cpp:602
constraint_samplers::IKConstraintSampler::project
bool project(moveit::core::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
Definition: default_constraint_samplers.cpp:667
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::VariableBounds
Definition: joint_model.h:118
ros::param::get
ROSCPP_DECL bool get(const std::string &key, bool &b)
moveit::core::VariableBounds::max_position_
double max_position_
Definition: joint_model.h:167
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
constraint_samplers::ConstraintSampler::jmg_
const moveit::core::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
Definition: constraint_sampler.h:322
constraint_samplers::JointConstraintSampler::JointInfo
An internal structure used for maintaining constraints on a particular joint.
Definition: default_constraint_samplers.h:195
constraint_samplers::JointConstraintSampler::clear
void clear() override
Clears all data from the constraint.
Definition: default_constraint_samplers.cpp:231
kinematic_constraints
Representation and evaluation of kinematic constraints.
Definition: kinematic_constraint.h:52
moveit::core::JointModel::getName
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:196
constraint_samplers::ConstraintSampler::verbose_
bool verbose_
True if verbosity is on.
Definition: constraint_sampler.h:327
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
constraint_samplers::IKConstraintSampler::sampling_pose_
IKSamplingPose sampling_pose_
Holder for the pose used for sampling.
Definition: default_constraint_samplers.h:554
constraint_samplers::IKConstraintSampler::random_number_generator_
random_numbers::RandomNumberGenerator random_number_generator_
Random generator used by the sampler.
Definition: default_constraint_samplers.h:553
moveit::core::JointModelGroup::getParentModel
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
Definition: joint_model_group.h:179
constraint_samplers::IKConstraintSampler::getLinkName
const std::string & getLinkName() const
Gets the link name associated with this sampler.
Definition: default_constraint_samplers.cpp:380
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::copyJointGroupPositions
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:757
constraint_samplers::IKConstraintSampler::ik_timeout_
double ik_timeout_
Holds the timeout associated with IK.
Definition: default_constraint_samplers.h:556
moveit::core::VariableBounds::min_position_
double min_position_
Definition: joint_model.h:166
kinematics::KinematicsBase::IKCallbackFn
boost::function< void(const geometry_msgs::Pose &, const std::vector< double > &, moveit_msgs::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
Definition: kinematics_base.h:154
constraint_samplers::IKSamplingPose::position_constraint_
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
Definition: default_constraint_samplers.h:312
constraint_samplers::JointConstraintSampler::random_number_generator_
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
Definition: default_constraint_samplers.h:230
constraint_samplers::JointConstraintSampler::uindex_
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
Definition: default_constraint_samplers.h:236
constraint_samplers::IKConstraintSampler::eef_to_ik_tip_transform_
Eigen::Isometry3d eef_to_ik_tip_transform_
Holds the transformation from end effector to IK tip frame.
Definition: default_constraint_samplers.h:562
moveit::core::RobotState::dirtyLinkTransforms
bool dirtyLinkTransforms() const
Definition: robot_state.h:1547
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:720
moveit::core::JointModel::getVariableBounds
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
Definition: joint_model.cpp:191
random_numbers::RandomNumberGenerator::quaternion
void quaternion(double value[4])
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
constraint_samplers::IKSamplingPose::IKSamplingPose
IKSamplingPose()
Empty constructor.
Definition: default_constraint_samplers.cpp:240
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
bound
template Interval< double > bound(const Interval< double > &i, const Interval< double > &other)
constraint_samplers::JointConstraintSampler::values_
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
Definition: default_constraint_samplers.h:237
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
constraint_samplers
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
Definition: constraint_sampler.h:51
moveit::core::LinkModel::getAssociatedFixedTransforms
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms. The returned transforms are ...
Definition: link_model.h:198
constraint_samplers::ConstraintSampler::frame_depends_
std::vector< std::string > frame_depends_
Holds the set of frames that must exist in the reference state to allow samples to be drawn.
Definition: constraint_sampler.h:324
constraint_samplers::ConstraintSampler::clear
virtual void clear()
Clears all data from the constraint.
Definition: constraint_sampler.cpp:49
constraint_samplers::ConstraintSampler::group_state_validity_callback_
moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_
Holds the callback for state validity.
Definition: constraint_sampler.h:326
constraint_samplers::JointConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &ks, unsigned int max_attempts) override
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
Definition: default_constraint_samplers.cpp:196
constraint_samplers::JointConstraintSampler::bounds_
std::vector< JointInfo > bounds_
The bounds for any joint with bounds that are more restrictive than the joint limits.
Definition: default_constraint_samplers.h:231
moveit::core::JointModelGroup::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
Definition: joint_model_group.h:384
kinematic_constraints::OrientationConstraint
Class for constraints on the orientation of a link.
Definition: kinematic_constraint.h:379
constraint_samplers::IKConstraintSampler::sample
bool sample(moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
Produces an IK sample.
Definition: default_constraint_samplers.cpp:596
moveit::core::LinkTransformMap
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:68
moveit::core::JointModelGroup::getJointModels
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Definition: joint_model_group.h:209
pr2_arm_kinematics::NO_IK_SOLUTION
static const int NO_IK_SOLUTION
Definition: pr2_arm_kinematics_plugin.h:95
constraint_samplers::IKConstraintSampler::loadIKSolver
bool loadIKSolver()
Performs checks and sets various internal values associated with the IK solver.
Definition: default_constraint_samplers.cpp:387
moveit::core::JointModelGroup::hasJointModel
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
Definition: joint_model_group.cpp:349
constraint_samplers::IKConstraintSampler::clear
void clear() override
Clears all data from the constraint.
Definition: default_constraint_samplers.cpp:275
moveit::core::Transforms::sameFrame
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:122
constraint_samplers::JointConstraintSampler::JointInfo::min_bound_
double min_bound_
Definition: default_constraint_samplers.h:223
constraint_samplers::IKConstraintSampler::transform_ik_
bool transform_ik_
True if the frame associated with the kinematic model is different than the base frame of the IK solv...
Definition: default_constraint_samplers.h:558
random_numbers::RandomNumberGenerator
point
std::chrono::system_clock::time_point point
bodies::BodyPtr
std::shared_ptr< Body > BodyPtr
constraint_samplers::ConstraintSampler::scene_
planning_scene::PlanningSceneConstPtr scene_
Holds the planning scene.
Definition: constraint_sampler.h:320
constraint_samplers::IKConstraintSampler::kb_
kinematics::KinematicsBaseConstPtr kb_
Holds the kinematics solver.
Definition: default_constraint_samplers.h:555
constraint_samplers::JointConstraintSampler::JointInfo::max_bound_
double max_bound_
Definition: default_constraint_samplers.h:224
constraint_samplers::IKConstraintSampler::callIK
bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, moveit::core::RobotState &state, bool use_as_seed)
Actually calls IK on the given pose, generating a random seed state.
Definition: default_constraint_samplers.cpp:681
moveit::core::JointModelGroup::getDefaultIKTimeout
double getDefaultIKTimeout() const
Get the default IK timeout.
Definition: joint_model_group.h:622
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1161
constraint_samplers::IKConstraintSampler::need_eef_to_ik_tip_transform_
bool need_eef_to_ik_tip_transform_
True if the tip frame of the inverse kinematic is different than the frame of the end effector.
Definition: default_constraint_samplers.h:560
moveit::core::JointModelGroup::getSolverInstance
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
Definition: joint_model_group.h:602
constraint_samplers::JointConstraintSampler::JointInfo::index_
std::size_t index_
Definition: default_constraint_samplers.h:225
constraint_samplers::JointConstraintSampler::configure
bool configure(const moveit_msgs::Constraints &constr) override
Configures a joint constraint given a Constraints message.
Definition: default_constraint_samplers.cpp:89
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
constraint_samplers::IKConstraintSampler::ik_frame_
std::string ik_frame_
Holds the base from of the IK solver.
Definition: default_constraint_samplers.h:557
random_numbers::RandomNumberGenerator::uniformInteger
int uniformInteger(int min, int max)
constraint_samplers::JointConstraintSampler::JointInfo::potentiallyAdjustMinMaxBounds
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
Definition: default_constraint_samplers.h:217
moveit::core::JointModelGroup::getKinematicsSolverJointBijection
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...
Definition: joint_model_group.h:634
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition: joint_model_group.h:475
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:80
moveit::core::RobotState::setJointGroupPositions
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:671
constraint_samplers::IKSamplingPose::orientation_constraint_
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
Definition: default_constraint_samplers.h:315
kinematic_constraints::JointConstraint
Class for handling single DOF joint constraints.
Definition: kinematic_constraint.h:234
constraint_samplers::IKConstraintSampler::validate
bool validate(moveit::core::RobotState &state) const
Definition: default_constraint_samplers.cpp:672
constraint_samplers::IKConstraintSampler::configure
bool configure(const moveit_msgs::Constraints &constr) override
Configures the IK constraint given a constraints message.
Definition: default_constraint_samplers.cpp:326
constraint_samplers::IKConstraintSampler::samplePose
bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const moveit::core::RobotState &ks, unsigned int max_attempts)
Returns a pose that falls within the constraint regions.
Definition: default_constraint_samplers.cpp:461
t
geometry_msgs::TransformStamped t
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
constraint_samplers::JointConstraintSampler::unbounded_
std::vector< const moveit::core::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
Definition: default_constraint_samplers.h:234
moveit::core::JointModelGroup::getVariableGroupIndex
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
Definition: joint_model_group.cpp:607
kinematic_constraints::PositionConstraint
Class for constraints on the XYZ position of a link.
Definition: kinematic_constraint.h:546
moveit::core::RobotModel::hasLinkModel
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
Definition: robot_model.cpp:1180
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
constraint_samplers::JointConstraintSampler::project
bool project(moveit::core::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
Definition: default_constraint_samplers.cpp:226


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 15 2022 02:24:54