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 namespace constraint_samplers
44 {
45 bool JointConstraintSampler::configure(const moveit_msgs::Constraints& constr)
46 {
47  // construct the constraints
48  std::vector<kinematic_constraints::JointConstraint> jc;
49  for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
50  {
52  if (j.configure(constr.joint_constraints[i]))
53  jc.push_back(j);
54  }
55 
56  return jc.empty() ? false : configure(jc);
57 }
58 
59 bool JointConstraintSampler::configure(const std::vector<kinematic_constraints::JointConstraint>& jc)
60 {
61  clear();
62 
63  if (!jmg_)
64  {
65  ROS_ERROR_NAMED("constraint_samplers", "NULL group specified for constraint sampler");
66  return false;
67  }
68 
69  // find and keep the constraints that operate on the group we sample
70  // also keep bounds for joints as convenient
71  std::map<std::string, JointInfo> bound_data;
72  bool some_valid_constraint = false;
73  for (std::size_t i = 0; i < jc.size(); ++i)
74  {
75  if (!jc[i].enabled())
76  continue;
77 
78  const robot_model::JointModel* jm = jc[i].getJointModel();
79  if (!jmg_->hasJointModel(jm->getName()))
80  continue;
81 
82  some_valid_constraint = true;
83 
84  const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(jc[i].getJointVariableName());
85  JointInfo ji;
86  std::map<std::string, JointInfo>::iterator it = bound_data.find(jc[i].getJointVariableName());
87  if (it != bound_data.end())
88  ji = it->second;
89  else
90  ji.index_ = jmg_->getVariableGroupIndex(jc[i].getJointVariableName());
92  std::max(joint_bounds.min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()),
93  std::min(joint_bounds.max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()));
94 
95  ROS_DEBUG_NAMED("constraint_samplers", "Bounds for %s JointConstraint are %g %g",
96  jc[i].getJointVariableName().c_str(), ji.min_bound_, ji.max_bound_);
97 
98  if (ji.min_bound_ > ji.max_bound_ + std::numeric_limits<double>::epsilon())
99  {
100  std::stringstream cs;
101  jc[i].print(cs);
102  ROS_ERROR_NAMED("constraint_samplers",
103  "The constraints for joint '%s' are such that "
104  "there are no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n",
105  jm->getName().c_str(), ji.min_bound_, ji.max_bound_);
106  clear();
107  return false;
108  }
109  bound_data[jc[i].getJointVariableName()] = ji;
110  }
111 
112  if (!some_valid_constraint)
113  {
114  ROS_WARN_NAMED("constraint_samplers", "No valid joint constraints");
115  return false;
116  }
117 
118  for (std::map<std::string, JointInfo>::iterator it = bound_data.begin(); it != bound_data.end(); ++it)
119  bounds_.push_back(it->second);
120 
121  // get a separate list of joints that are not bounded; we will sample these randomly
122  const std::vector<const robot_model::JointModel*>& joints = jmg_->getJointModels();
123  for (std::size_t i = 0; i < joints.size(); ++i)
124  if (bound_data.find(joints[i]->getName()) == bound_data.end() && joints[i]->getVariableCount() > 0 &&
125  joints[i]->getMimic() == nullptr)
126  {
127  // check if all the vars of the joint are found in bound_data instead
128  const std::vector<std::string>& vars = joints[i]->getVariableNames();
129  if (vars.size() > 1)
130  {
131  bool all_found = true;
132  for (std::size_t j = 0; j < vars.size(); ++j)
133  if (bound_data.find(vars[j]) == bound_data.end())
134  {
135  all_found = false;
136  break;
137  }
138  if (all_found)
139  continue;
140  }
141  unbounded_.push_back(joints[i]);
142  // Get the first variable name of this joint and find its index position in the planning group
143  uindex_.push_back(jmg_->getVariableGroupIndex(vars[0]));
144  }
145  values_.resize(jmg_->getVariableCount());
146  is_valid_ = true;
147  return true;
148 }
149 
151  const robot_state::RobotState& /* reference_state */,
152  unsigned int /* max_attempts */)
153 {
154  if (!is_valid_)
155  {
156  ROS_WARN_NAMED("constraint_samplers", "JointConstraintSampler not configured, won't sample");
157  return false;
158  }
159 
160  // sample the unbounded joints first (in case some joint variables are bounded)
161  std::vector<double> v;
162  for (std::size_t i = 0; i < unbounded_.size(); ++i)
163  {
164  v.resize(unbounded_[i]->getVariableCount());
165  unbounded_[i]->getVariableRandomPositions(random_number_generator_, &v[0]);
166  for (std::size_t j = 0; j < v.size(); ++j)
167  values_[uindex_[i] + j] = v[j];
168  }
169 
170  // enforce the constraints for the constrained components (could be all of them)
171  for (std::size_t i = 0; i < bounds_.size(); ++i)
172  values_[bounds_[i].index_] = random_number_generator_.uniformReal(bounds_[i].min_bound_, bounds_[i].max_bound_);
173 
175 
176  // we are always successful
177  return true;
178 }
179 
180 bool JointConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
181 {
182  return sample(state, state, max_attempts);
183 }
184 
186 {
188  bounds_.clear();
189  unbounded_.clear();
190  uindex_.clear();
191  values_.clear();
192 }
193 
195 {
196 }
197 
199  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
200 {
201 }
202 
204  : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
205 {
206 }
207 
210  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
211  , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
212 {
213 }
214 
215 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc) : position_constraint_(pc)
216 {
217 }
218 
219 IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc) : orientation_constraint_(oc)
220 {
221 }
222 
223 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  ROS_WARN_NAMED("constraint_samplers", "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  ROS_ERROR_NAMED("constraint_samplers",
260  "Position and orientation constraints need to be specified for the same link "
261  "in order to use IK-based sampling");
262  return false;
263  }
264 
265  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->mobileReferenceFrame())
266  frame_depends_.push_back(sampling_pose_.position_constraint_->getReferenceFrame());
267  if (sampling_pose_.orientation_constraint_ && sampling_pose_.orientation_constraint_->mobileReferenceFrame())
268  frame_depends_.push_back(sampling_pose_.orientation_constraint_->getReferenceFrame());
269  kb_ = jmg_->getSolverInstance();
270  if (!kb_)
271  {
272  ROS_WARN_NAMED("constraint_samplers", "No solver instance in setup");
273  is_valid_ = false;
274  return false;
275  }
276  is_valid_ = loadIKSolver();
277  return is_valid_;
278 }
279 
280 bool IKConstraintSampler::configure(const moveit_msgs::Constraints& constr)
281 {
282  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
283  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
284  if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
285  {
286  kinematic_constraints::PositionConstraintPtr pc(
287  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
288  kinematic_constraints::OrientationConstraintPtr oc(
289  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
290  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
291  oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
292  return configure(IKSamplingPose(pc, oc));
293  }
294 
295  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
296  {
297  kinematic_constraints::PositionConstraintPtr pc(
298  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
299  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()))
300  return configure(IKSamplingPose(pc));
301  }
302 
303  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
304  {
305  kinematic_constraints::OrientationConstraintPtr oc(
306  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
307  if (oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
308  return configure(IKSamplingPose(oc));
309  }
310  return false;
311 }
312 
314 {
315  double v = 1.0;
316  if (sampling_pose_.position_constraint_)
317  {
318  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
319  double vol = 0;
320  for (std::size_t i = 0; i < b.size(); ++i)
321  vol += b[i]->computeVolume();
322  if (!b.empty())
323  v *= vol;
324  }
325 
326  if (sampling_pose_.orientation_constraint_)
327  v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
328  sampling_pose_.orientation_constraint_->getYAxisTolerance() *
329  sampling_pose_.orientation_constraint_->getZAxisTolerance();
330  return v;
331 }
332 
333 const std::string& IKConstraintSampler::getLinkName() const
334 {
335  if (sampling_pose_.orientation_constraint_)
336  return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
337  return sampling_pose_.position_constraint_->getLinkModel()->getName();
338 }
339 
341 {
342  if (!kb_)
343  {
344  ROS_ERROR_NAMED("constraint_samplers", "No IK solver");
345  return false;
346  }
347 
348  // check if we need to transform the request into the coordinate frame expected by IK
349  ik_frame_ = kb_->getBaseFrame();
350  transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame());
351  if (!ik_frame_.empty() && ik_frame_[0] == '/')
352  ik_frame_.erase(ik_frame_.begin());
353  if (transform_ik_)
354  if (!jmg_->getParentModel().hasLinkModel(ik_frame_))
355  {
356  ROS_ERROR_NAMED("constraint_samplers",
357  "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. "
358  "Ignoring transformation (IK may fail)",
359  ik_frame_.c_str());
360  transform_ik_ = false;
361  }
362 
363  // check if IK is performed for the desired link
364  bool wrong_link = false;
365  if (sampling_pose_.position_constraint_)
366  {
367  const moveit::core::LinkModel* lm = sampling_pose_.position_constraint_->getLinkModel();
368  if (!moveit::core::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
369  {
370  wrong_link = true;
372  for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
373  if (moveit::core::Transforms::sameFrame(it->first->getName(), kb_->getTipFrame()))
374  {
375  eef_to_ik_tip_transform_ = it->second;
376  need_eef_to_ik_tip_transform_ = true;
377  wrong_link = false;
378  break;
379  }
380  }
381  }
382 
383  if (!wrong_link && sampling_pose_.orientation_constraint_)
384  {
385  const moveit::core::LinkModel* lm = sampling_pose_.orientation_constraint_->getLinkModel();
386  if (!robot_state::Transforms::sameFrame(kb_->getTipFrame(), lm->getName()))
387  {
388  wrong_link = true;
390  for (moveit::core::LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
391  if (moveit::core::Transforms::sameFrame(it->first->getName(), kb_->getTipFrame()))
392  {
393  eef_to_ik_tip_transform_ = it->second;
394  need_eef_to_ik_tip_transform_ = true;
395  wrong_link = false;
396  break;
397  }
398  }
399  }
400 
401  if (wrong_link)
402  {
403  ROS_ERROR_NAMED("constraint_samplers",
404  "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
405  sampling_pose_.position_constraint_ ?
406  sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
407  sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
408  kb_->getTipFrame().c_str());
409  return false;
410  }
411  return true;
412 }
413 
414 bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks,
415  unsigned int max_attempts)
416 {
417  if (ks.dirtyLinkTransforms())
418  {
419  // samplePose below requires accurate transforms
420  ROS_ERROR_NAMED("constraint_samplers",
421  "IKConstraintSampler received dirty robot state, but valid transforms are required. "
422  "Failing.");
423  return false;
424  }
425 
426  if (sampling_pose_.position_constraint_)
427  {
428  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
429  if (!b.empty())
430  {
431  bool found = false;
432  std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
433  for (std::size_t i = 0; i < b.size(); ++i)
434  if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
435  {
436  found = true;
437  break;
438  }
439  if (!found)
440  {
441  ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region");
442  return false;
443  }
444  }
445  else
446  {
447  ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region. "
448  "Constraint region is empty when it should not be.");
449  return false;
450  }
451 
452  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
453  // model
454  if (sampling_pose_.position_constraint_->mobileReferenceFrame())
455  pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
456  }
457  else
458  {
459  // do FK for rand state
460  robot_state::RobotState tempState(ks);
461  tempState.setToRandomPositions(jmg_);
462  pos = tempState.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
463  }
464 
465  if (sampling_pose_.orientation_constraint_)
466  {
467  // sample a rotation matrix within the allowed bounds
468  double angle_x =
469  2.0 * (random_number_generator_.uniform01() - 0.5) *
470  (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
471  double angle_y =
472  2.0 * (random_number_generator_.uniform01() - 0.5) *
473  (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
474  double angle_z =
475  2.0 * (random_number_generator_.uniform01() - 0.5) *
476  (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
477  Eigen::Affine3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
478  Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
479  Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
480  Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
481  quat = Eigen::Quaterniond(reqr.linear());
482 
483  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
484  // model
485  if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
486  {
487  const Eigen::Affine3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
488  Eigen::Affine3d rt(t.linear() * quat.toRotationMatrix());
489  quat = Eigen::Quaterniond(rt.linear());
490  }
491  }
492  else
493  {
494  // sample a random orientation
495  double q[4];
496  random_number_generator_.quaternion(q);
497  quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
498  }
499 
500  // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
501  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
502  // the rotation matrix that corresponds to the desired orientation
503  pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
504 
505  return true;
506 }
507 
508 namespace
509 {
510 void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
512  const geometry_msgs::Pose& /*unused*/, const std::vector<double>& ik_sol,
513  moveit_msgs::MoveItErrorCodes& error_code)
514 {
515  const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
516  std::vector<double> solution(bij.size());
517  for (std::size_t i = 0; i < bij.size(); ++i)
518  solution[i] = ik_sol[bij[i]];
519  if (constraint(state, jmg, &solution[0]))
520  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
521  else
523 }
524 }
525 
527  unsigned int max_attempts)
528 {
529  return sampleHelper(state, reference_state, max_attempts, false);
530 }
531 
533  unsigned int max_attempts, bool project)
534 {
535  if (!is_valid_)
536  {
537  ROS_WARN_NAMED("constraint_samplers", "IKConstraintSampler not configured, won't sample");
538  return false;
539  }
540 
541  kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback;
542  if (group_state_validity_callback_)
543  adapted_ik_validity_callback =
544  boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3);
545 
546  for (unsigned int a = 0; a < max_attempts; ++a)
547  {
548  // sample a point in the constraint region
549  Eigen::Vector3d point;
550  Eigen::Quaterniond quat;
551  if (!samplePose(point, quat, reference_state, max_attempts))
552  {
553  if (verbose_)
554  ROS_INFO_NAMED("constraint_samplers", "IK constraint sampler was unable to produce a pose to run IK for");
555  return false;
556  }
557 
558  // we now have the transform we wish to perform IK for, in the planning frame
559  if (transform_ik_)
560  {
561  // we need to convert this transform to the frame expected by the IK solver
562  // both the planning frame and the frame for the IK are assumed to be robot links
563  Eigen::Affine3d ikq(Eigen::Translation3d(point) * quat.toRotationMatrix());
564  ikq = reference_state.getFrameTransform(ik_frame_).inverse(Eigen::Isometry) * ikq;
565  point = ikq.translation();
566  quat = Eigen::Quaterniond(ikq.linear());
567  }
568 
569  if (need_eef_to_ik_tip_transform_)
570  {
571  // After sampling the pose needs to be transformed to the ik chain tip
572  Eigen::Affine3d ikq(Eigen::Translation3d(point) * quat.toRotationMatrix());
573  ikq = ikq * eef_to_ik_tip_transform_;
574  point = ikq.translation();
575  quat = Eigen::Quaterniond(ikq.linear());
576  }
577 
578  geometry_msgs::Pose ik_query;
579  ik_query.position.x = point.x();
580  ik_query.position.y = point.y();
581  ik_query.position.z = point.z();
582  ik_query.orientation.x = quat.x();
583  ik_query.orientation.y = quat.y();
584  ik_query.orientation.z = quat.z();
585  ik_query.orientation.w = quat.w();
586 
587  if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0))
588  return true;
589  }
590  return false;
591 }
592 
593 bool IKConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
594 {
595  return sampleHelper(state, state, max_attempts, true);
596 }
597 
599 {
600  state.update();
601  return (!sampling_pose_.orientation_constraint_ ||
602  sampling_pose_.orientation_constraint_->decide(state, verbose_).satisfied) &&
603  (!sampling_pose_.position_constraint_ ||
604  sampling_pose_.position_constraint_->decide(state, verbose_).satisfied);
605 }
606 
607 bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query,
608  const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback,
609  double timeout, robot_state::RobotState& state, bool use_as_seed)
610 {
611  const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
612  std::vector<double> seed(ik_joint_bijection.size(), 0.0);
613  std::vector<double> vals;
614 
615  if (use_as_seed)
616  state.copyJointGroupPositions(jmg_, vals);
617  else
618  // sample a seed value
619  jmg_->getVariableRandomPositions(random_number_generator_, vals);
620 
621  assert(vals.size() == ik_joint_bijection.size());
622  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
623  seed[i] = vals[ik_joint_bijection[i]];
624 
625  std::vector<double> ik_sol;
626  moveit_msgs::MoveItErrorCodes error;
627 
628  if (adapted_ik_validity_callback ?
629  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
630  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
631  {
632  assert(ik_sol.size() == ik_joint_bijection.size());
633  std::vector<double> solution(ik_joint_bijection.size());
634  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
635  solution[ik_joint_bijection[i]] = ik_sol[i];
636  state.setJointGroupPositions(jmg_, solution);
637 
638  return validate(state);
639  }
640  else
641  {
643  error.val != moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE &&
645  ROS_ERROR_NAMED("constraint_samplers", "IK solver failed with error %d", error.val);
646  else if (verbose_)
647  ROS_INFO_NAMED("constraint_samplers", "IK failed");
648  }
649  return false;
650 }
651 
652 } // end of namespace constraint_samplers
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...
#define ROS_INFO_NAMED(name,...)
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.
#define ROS_WARN_NAMED(name,...)
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1368
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:64
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.
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.
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
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:666
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...
#define ROS_DEBUG_NAMED(name,...)
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:1445
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.
const robot_model::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
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:618
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.
#define ROS_ERROR_NAMED(name,...)
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:123
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.
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.
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 Wed Jul 10 2019 04:03:05