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 <boost/bind.hpp>
40 
41 namespace constraint_samplers
42 {
43 bool 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 
57 bool JointConstraintSampler::configure(const std::vector<kinematic_constraints::JointConstraint>& jc)
58 {
59  clear();
60 
61  if (!jmg_)
62  {
63  ROS_ERROR_NAMED("constraint_samplers", "NULL group specified for constraint sampler");
64  return false;
65  }
66 
67  // find and keep the constraints that operate on the group we sample
68  // also keep bounds for joints as convenient
69  std::map<std::string, JointInfo> bound_data;
70  bool some_valid_constraint = false;
71  for (std::size_t i = 0; i < jc.size(); ++i)
72  {
73  if (!jc[i].enabled())
74  continue;
75 
76  const robot_model::JointModel* jm = jc[i].getJointModel();
77  if (!jmg_->hasJointModel(jm->getName()))
78  continue;
79 
80  some_valid_constraint = true;
81 
82  const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(jc[i].getJointVariableName());
83  JointInfo ji;
84  std::map<std::string, JointInfo>::iterator it = bound_data.find(jc[i].getJointVariableName());
85  if (it != bound_data.end())
86  ji = it->second;
87  else
88  ji.index_ = jmg_->getVariableGroupIndex(jc[i].getJointVariableName());
90  std::max(joint_bounds.min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()),
91  std::min(joint_bounds.max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()));
92 
93  ROS_DEBUG_NAMED("constraint_samplers", "Bounds for %s JointConstraint are %g %g",
94  jc[i].getJointVariableName().c_str(), ji.min_bound_, ji.max_bound_);
95 
96  if (ji.min_bound_ > ji.max_bound_ + std::numeric_limits<double>::epsilon())
97  {
98  std::stringstream cs;
99  jc[i].print(cs);
100  ROS_ERROR_NAMED("constraint_samplers",
101  "The constraints for joint '%s' are such that "
102  "there are no possible values for the 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  ROS_WARN_NAMED("constraint_samplers", "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() == nullptr)
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  ROS_WARN_NAMED("constraint_samplers", "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 
178 bool JointConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
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 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc) : position_constraint_(pc)
214 {
215 }
216 
217 IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc) : orientation_constraint_(oc)
218 {
219 }
220 
221 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
222  const kinematic_constraints::OrientationConstraintPtr& oc)
224 {
225 }
226 
228 {
230  kb_.reset();
231  ik_frame_ = "";
232  transform_ik_ = false;
233  eef_to_ik_tip_transform_ = Eigen::Isometry3d::Identity();
234  need_eef_to_ik_tip_transform_ = false;
235 }
236 
238 {
239  clear();
241  return false;
242  if ((!sp.orientation_constraint_ && !sp.position_constraint_->enabled()) ||
243  (!sp.position_constraint_ && !sp.orientation_constraint_->enabled()) ||
245  !sp.orientation_constraint_->enabled()))
246  {
247  ROS_WARN_NAMED("constraint_samplers", "No enabled constraints in sampling pose");
248  return false;
249  }
250 
251  sampling_pose_ = sp;
252  ik_timeout_ = jmg_->getDefaultIKTimeout();
253  if (sampling_pose_.position_constraint_ && sampling_pose_.orientation_constraint_)
254  if (sampling_pose_.position_constraint_->getLinkModel()->getName() !=
255  sampling_pose_.orientation_constraint_->getLinkModel()->getName())
256  {
257  ROS_ERROR_NAMED("constraint_samplers",
258  "Position and orientation constraints need to be specified for the same link "
259  "in order to use IK-based sampling");
260  return false;
261  }
262 
263  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->mobileReferenceFrame())
264  frame_depends_.push_back(sampling_pose_.position_constraint_->getReferenceFrame());
265  if (sampling_pose_.orientation_constraint_ && sampling_pose_.orientation_constraint_->mobileReferenceFrame())
266  frame_depends_.push_back(sampling_pose_.orientation_constraint_->getReferenceFrame());
267  kb_ = jmg_->getSolverInstance();
268  if (!kb_)
269  {
270  ROS_WARN_NAMED("constraint_samplers", "No solver instance in setup");
271  is_valid_ = false;
272  return false;
273  }
274  is_valid_ = loadIKSolver();
275  return is_valid_;
276 }
277 
278 bool IKConstraintSampler::configure(const moveit_msgs::Constraints& constr)
279 {
280  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
281  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
282  if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
283  {
284  kinematic_constraints::PositionConstraintPtr pc(
285  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
286  kinematic_constraints::OrientationConstraintPtr oc(
287  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
288  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
289  oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
290  return configure(IKSamplingPose(pc, oc));
291  }
292 
293  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
294  {
295  kinematic_constraints::PositionConstraintPtr pc(
296  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
297  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()))
298  return configure(IKSamplingPose(pc));
299  }
300 
301  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
302  {
303  kinematic_constraints::OrientationConstraintPtr oc(
304  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
305  if (oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
306  return configure(IKSamplingPose(oc));
307  }
308  return false;
309 }
310 
312 {
313  double v = 1.0;
314  if (sampling_pose_.position_constraint_)
315  {
316  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
317  double vol = 0;
318  for (std::size_t i = 0; i < b.size(); ++i)
319  vol += b[i]->computeVolume();
320  if (!b.empty())
321  v *= vol;
322  }
323 
324  if (sampling_pose_.orientation_constraint_)
325  v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
326  sampling_pose_.orientation_constraint_->getYAxisTolerance() *
327  sampling_pose_.orientation_constraint_->getZAxisTolerance();
328  return v;
329 }
330 
331 const std::string& IKConstraintSampler::getLinkName() const
332 {
333  if (sampling_pose_.orientation_constraint_)
334  return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
335  return sampling_pose_.position_constraint_->getLinkModel()->getName();
336 }
337 
339 {
340  if (!kb_)
341  {
342  ROS_ERROR_NAMED("constraint_samplers", "No IK solver");
343  return false;
344  }
345 
346  // check if we need to transform the request into the coordinate frame expected by IK
347  ik_frame_ = kb_->getBaseFrame();
348  transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame());
349  if (!ik_frame_.empty() && ik_frame_[0] == '/')
350  ik_frame_.erase(ik_frame_.begin());
351  if (transform_ik_)
352  if (!jmg_->getParentModel().hasLinkModel(ik_frame_))
353  {
354  ROS_ERROR_NAMED("constraint_samplers",
355  "The IK solver expects requests in frame '%s' but this frame is not known to the sampler. "
356  "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  ROS_ERROR_NAMED("constraint_samplers",
402  "IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
403  sampling_pose_.position_constraint_ ?
404  sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
405  sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
406  kb_->getTipFrame().c_str());
407  return false;
408  }
409  return true;
410 }
411 
412 bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks,
413  unsigned int max_attempts)
414 {
415  if (ks.dirtyLinkTransforms())
416  {
417  // samplePose below requires accurate transforms
418  ROS_ERROR_NAMED("constraint_samplers",
419  "IKConstraintSampler received dirty robot state, but valid transforms are required. "
420  "Failing.");
421  return false;
422  }
423 
424  if (sampling_pose_.position_constraint_)
425  {
426  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
427  if (!b.empty())
428  {
429  bool found = false;
430  std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
431  for (std::size_t i = 0; i < b.size(); ++i)
432  if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
433  {
434  found = true;
435  break;
436  }
437  if (!found)
438  {
439  ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region");
440  return false;
441  }
442  }
443  else
444  {
445  ROS_ERROR_NAMED("constraint_samplers", "Unable to sample a point inside the constraint region. "
446  "Constraint region is empty when it should not be.");
447  return false;
448  }
449 
450  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
451  // model
452  if (sampling_pose_.position_constraint_->mobileReferenceFrame())
453  pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
454  }
455  else
456  {
457  // do FK for rand state
458  robot_state::RobotState temp_state(ks);
459  temp_state.setToRandomPositions(jmg_);
460  pos = temp_state.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
461  }
462 
463  if (sampling_pose_.orientation_constraint_)
464  {
465  // sample a rotation matrix within the allowed bounds
466  double angle_x =
467  2.0 * (random_number_generator_.uniform01() - 0.5) *
468  (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
469  double angle_y =
470  2.0 * (random_number_generator_.uniform01() - 0.5) *
471  (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
472  double angle_z =
473  2.0 * (random_number_generator_.uniform01() - 0.5) *
474  (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
475  Eigen::Isometry3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
476  Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
477  Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
478  Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
479  quat = Eigen::Quaterniond(reqr.rotation());
480 
481  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
482  // model
483  if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
484  {
485  const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
486  Eigen::Isometry3d rt(t.rotation() * quat);
487  quat = Eigen::Quaterniond(rt.rotation());
488  }
489  }
490  else
491  {
492  // sample a random orientation
493  double q[4];
494  random_number_generator_.quaternion(q);
495  quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
496  }
497 
498  // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
499  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
500  // the rotation matrix that corresponds to the desired orientation
501  pos = pos - quat * sampling_pose_.position_constraint_->getLinkOffset();
502 
503  return true;
504 }
505 
506 namespace
507 {
508 void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
510  const geometry_msgs::Pose& /*unused*/, 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 } // namespace
523 
525  unsigned int max_attempts)
526 {
527  return sampleHelper(state, reference_state, max_attempts, false);
528 }
529 
531  unsigned int max_attempts, bool project)
532 {
533  if (!is_valid_)
534  {
535  ROS_WARN_NAMED("constraint_samplers", "IKConstraintSampler not configured, won't sample");
536  return false;
537  }
538 
539  kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback;
540  if (group_state_validity_callback_)
541  adapted_ik_validity_callback =
542  boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3);
543 
544  for (unsigned int a = 0; a < max_attempts; ++a)
545  {
546  // sample a point in the constraint region
547  Eigen::Vector3d point;
548  Eigen::Quaterniond quat;
549  if (!samplePose(point, quat, reference_state, max_attempts))
550  {
551  if (verbose_)
552  ROS_INFO_NAMED("constraint_samplers", "IK constraint sampler was unable to produce a pose to run IK for");
553  return false;
554  }
555 
556  // we now have the transform we wish to perform IK for, in the planning frame
557  if (transform_ik_)
558  {
559  // we need to convert this transform to the frame expected by the IK solver
560  // both the planning frame and the frame for the IK are assumed to be robot links
561  Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
562  ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq;
563  point = ikq.translation();
564  quat = Eigen::Quaterniond(ikq.rotation());
565  }
566 
567  if (need_eef_to_ik_tip_transform_)
568  {
569  // After sampling the pose needs to be transformed to the ik chain tip
570  Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
571  ikq = ikq * eef_to_ik_tip_transform_;
572  point = ikq.translation();
573  quat = Eigen::Quaterniond(ikq.rotation());
574  }
575 
576  geometry_msgs::Pose ik_query;
577  ik_query.position.x = point.x();
578  ik_query.position.y = point.y();
579  ik_query.position.z = point.z();
580  ik_query.orientation.x = quat.x();
581  ik_query.orientation.y = quat.y();
582  ik_query.orientation.z = quat.z();
583  ik_query.orientation.w = quat.w();
584 
585  if (callIK(ik_query, adapted_ik_validity_callback, ik_timeout_, state, project && a == 0))
586  return true;
587  }
588  return false;
589 }
590 
591 bool IKConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
592 {
593  return sampleHelper(state, state, max_attempts, true);
594 }
595 
597 {
598  state.update();
599  return (!sampling_pose_.orientation_constraint_ ||
600  sampling_pose_.orientation_constraint_->decide(state, verbose_).satisfied) &&
601  (!sampling_pose_.position_constraint_ ||
602  sampling_pose_.position_constraint_->decide(state, verbose_).satisfied);
603 }
604 
605 bool IKConstraintSampler::callIK(const geometry_msgs::Pose& ik_query,
606  const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback,
607  double timeout, robot_state::RobotState& state, bool use_as_seed)
608 {
609  const std::vector<unsigned int>& ik_joint_bijection = jmg_->getKinematicsSolverJointBijection();
610  std::vector<double> seed(ik_joint_bijection.size(), 0.0);
611  std::vector<double> vals;
612 
613  if (use_as_seed)
614  state.copyJointGroupPositions(jmg_, vals);
615  else
616  // sample a seed value
617  jmg_->getVariableRandomPositions(random_number_generator_, vals);
618 
619  assert(vals.size() == ik_joint_bijection.size());
620  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
621  seed[i] = vals[ik_joint_bijection[i]];
622 
623  std::vector<double> ik_sol;
624  moveit_msgs::MoveItErrorCodes error;
625 
626  if (adapted_ik_validity_callback ?
627  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, adapted_ik_validity_callback, error) :
628  kb_->searchPositionIK(ik_query, seed, timeout, ik_sol, error))
629  {
630  assert(ik_sol.size() == ik_joint_bijection.size());
631  std::vector<double> solution(ik_joint_bijection.size());
632  for (std::size_t i = 0; i < ik_joint_bijection.size(); ++i)
633  solution[ik_joint_bijection[i]] = ik_sol[i];
634  state.setJointGroupPositions(jmg_, solution);
635 
636  return validate(state);
637  }
638  else
639  {
641  error.val != moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE &&
643  ROS_ERROR_NAMED("constraint_samplers", "IK solver failed with error %d", error.val);
644  else if (verbose_)
645  ROS_INFO_NAMED("constraint_samplers", "IK failed");
646  }
647  return false;
648 }
649 
650 } // end of namespace constraint_samplers
bool validate(robot_state::RobotState &state) const
bool configure(const moveit_msgs::Constraints &constr) override
Configures a joint constraint given a Constraints message.
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...
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:69
#define ROS_INFO_NAMED(name,...)
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.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
#define ROS_WARN_NAMED(name,...)
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...
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts) override
Produces an IK sample.
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
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)
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:57
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
void clear() override
Clears all data from the constraint.
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
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.
geometry_msgs::TransformStamped t
bool dirtyLinkTransforms() const
Definition: robot_state.h:1518
bool project(robot_state::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
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.
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.
double uniformReal(double lower_bound, double upper_bound)
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id)
Get the transformation matrix from the model frame to the frame identified by frame_id.
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:687
Class for constraints on the XYZ position of a link.
bool configure(const moveit_msgs::Constraints &constr) override
Configures the IK constraint given a constraints message.
const robot_model::JointModelGroup *const jmg_
Holds the joint model group associated with this constraint.
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
void clear() override
Clears all data from the constraint.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1441
bool project(robot_state::RobotState &state, unsigned int max_attempts) override
Project a sample given the constraints, updating the joint state group. This function allows the para...
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:639
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,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
bool sample(robot_state::RobotState &state, const robot_state::RobotState &ks, unsigned int max_attempts) override
Samples given the constraints, populating state. This function allows the parameter max_attempts to b...
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
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:63
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.
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Jan 14 2021 03:57:43