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