default_constraint_samplers.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 #include <set>
39 #include <cassert>
41 #include <boost/bind.hpp>
42 
43 bool constraint_samplers::JointConstraintSampler::configure(const moveit_msgs::Constraints& constr)
44 {
45  // construct the constraints
46  std::vector<kinematic_constraints::JointConstraint> jc;
47  for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
48  {
50  if (j.configure(constr.joint_constraints[i]))
51  jc.push_back(j);
52  }
53 
54  return jc.empty() ? false : configure(jc);
55 }
56 
58  const std::vector<kinematic_constraints::JointConstraint>& jc)
59 {
60  clear();
61 
62  if (!jmg_)
63  {
64  logError("NULL group specified for constraint sampler");
65  return false;
66  }
67 
68  // find and keep the constraints that operate on the group we sample
69  // also keep bounds for joints as convenient
70  std::map<std::string, JointInfo> bound_data;
71  bool some_valid_constraint = false;
72  for (std::size_t i = 0; i < jc.size(); ++i)
73  {
74  if (!jc[i].enabled())
75  continue;
76 
77  const robot_model::JointModel* jm = jc[i].getJointModel();
78  if (!jmg_->hasJointModel(jm->getName()))
79  continue;
80 
81  some_valid_constraint = true;
82 
83  const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(jc[i].getJointVariableName());
84  JointInfo ji;
85  std::map<std::string, JointInfo>::iterator it = bound_data.find(jc[i].getJointVariableName());
86  if (it != bound_data.end())
87  ji = it->second;
88  else
89  ji.index_ = jmg_->getVariableGroupIndex(jc[i].getJointVariableName());
91  std::max(joint_bounds.min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()),
92  std::min(joint_bounds.max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove()));
93 
94  logDebug("Bounds for %s JointConstraint are %g %g", jc[i].getJointVariableName().c_str(), ji.min_bound_,
95  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  logError("The constraints for joint '%s' are such that there are no possible values for the joint: min_bound: "
102  "%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  logWarn("No valid joint constraints");
113  return false;
114  }
115 
116  for (std::map<std::string, JointInfo>::iterator it = bound_data.begin(); it != bound_data.end(); ++it)
117  bounds_.push_back(it->second);
118 
119  // get a separate list of joints that are not bounded; we will sample these randomly
120  const std::vector<const robot_model::JointModel*>& joints = jmg_->getJointModels();
121  for (std::size_t i = 0; i < joints.size(); ++i)
122  if (bound_data.find(joints[i]->getName()) == bound_data.end() && joints[i]->getVariableCount() > 0 &&
123  joints[i]->getMimic() == NULL)
124  {
125  // check if all the vars of the joint are found in bound_data instead
126  const std::vector<std::string>& vars = joints[i]->getVariableNames();
127  if (vars.size() > 1)
128  {
129  bool all_found = true;
130  for (std::size_t j = 0; j < vars.size(); ++j)
131  if (bound_data.find(vars[j]) == bound_data.end())
132  {
133  all_found = false;
134  break;
135  }
136  if (all_found)
137  continue;
138  }
139  unbounded_.push_back(joints[i]);
140  // Get the first variable name of this joint and find its index position in the planning group
141  uindex_.push_back(jmg_->getVariableGroupIndex(vars[0]));
142  }
143  values_.resize(jmg_->getVariableCount());
144  is_valid_ = true;
145  return true;
146 }
147 
149  const robot_state::RobotState& /* reference_state */,
150  unsigned int /* max_attempts */)
151 {
152  if (!is_valid_)
153  {
154  logWarn("JointConstraintSampler not configured, won't sample");
155  return false;
156  }
157 
158  // sample the unbounded joints first (in case some joint variables are bounded)
159  std::vector<double> v;
160  for (std::size_t i = 0; i < unbounded_.size(); ++i)
161  {
162  v.resize(unbounded_[i]->getVariableCount());
163  unbounded_[i]->getVariableRandomPositions(random_number_generator_, &v[0]);
164  for (std::size_t j = 0; j < v.size(); ++j)
165  values_[uindex_[i] + j] = v[j];
166  }
167 
168  // enforce the constraints for the constrained components (could be all of them)
169  for (std::size_t i = 0; i < bounds_.size(); ++i)
170  values_[bounds_[i].index_] = random_number_generator_.uniformReal(bounds_[i].min_bound_, bounds_[i].max_bound_);
171 
173 
174  // we are always successful
175  return true;
176 }
177 
179 {
180  return sample(state, state, max_attempts);
181 }
182 
184 {
186  bounds_.clear();
187  unbounded_.clear();
188  uindex_.clear();
189  values_.clear();
190 }
191 
193 {
194 }
195 
197  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
198 {
199 }
200 
202  : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
203 {
204 }
205 
208  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
209  , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
210 {
211 }
212 
213 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc)
215 {
216 }
217 
218 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc)
220 {
221 }
222 
223 constraint_samplers::IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
224  const kinematic_constraints::OrientationConstraintPtr& oc)
226 {
227 }
228 
230 {
232  kb_.reset();
233  ik_frame_ = "";
234  transform_ik_ = false;
235  eef_to_ik_tip_transform_ = Eigen::Affine3d::Identity();
236  need_eef_to_ik_tip_transform_ = false;
237 }
238 
240 {
241  clear();
243  return false;
244  if ((!sp.orientation_constraint_ && !sp.position_constraint_->enabled()) ||
245  (!sp.position_constraint_ && !sp.orientation_constraint_->enabled()) ||
247  !sp.orientation_constraint_->enabled()))
248  {
249  logWarn("No enabled constraints in sampling pose");
250  return false;
251  }
252 
253  sampling_pose_ = sp;
254  ik_timeout_ = jmg_->getDefaultIKTimeout();
255  if (sampling_pose_.position_constraint_ && sampling_pose_.orientation_constraint_)
256  if (sampling_pose_.position_constraint_->getLinkModel()->getName() !=
257  sampling_pose_.orientation_constraint_->getLinkModel()->getName())
258  {
259  logError("Position and orientation constraints need to be specified for the same link in order to use IK-based "
260  "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  logWarn("No solver instance in setup");
272  is_valid_ = false;
273  return false;
274  }
275  is_valid_ = loadIKSolver();
276  return is_valid_;
277 }
278 
279 bool constraint_samplers::IKConstraintSampler::configure(const moveit_msgs::Constraints& constr)
280 {
281  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
282  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
283  if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
284  {
285  kinematic_constraints::PositionConstraintPtr pc(
286  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
287  kinematic_constraints::OrientationConstraintPtr oc(
288  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
289  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()) &&
290  oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
291  return configure(IKSamplingPose(pc, oc));
292  }
293 
294  for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
295  {
296  kinematic_constraints::PositionConstraintPtr pc(
297  new kinematic_constraints::PositionConstraint(scene_->getRobotModel()));
298  if (pc->configure(constr.position_constraints[p], scene_->getTransforms()))
299  return configure(IKSamplingPose(pc));
300  }
301 
302  for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
303  {
304  kinematic_constraints::OrientationConstraintPtr oc(
305  new kinematic_constraints::OrientationConstraint(scene_->getRobotModel()));
306  if (oc->configure(constr.orientation_constraints[o], scene_->getTransforms()))
307  return configure(IKSamplingPose(oc));
308  }
309  return false;
310 }
311 
313 {
314  double v = 1.0;
315  if (sampling_pose_.position_constraint_)
316  {
317  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
318  double vol = 0;
319  for (std::size_t i = 0; i < b.size(); ++i)
320  vol += b[i]->computeVolume();
321  if (!b.empty())
322  v *= vol;
323  }
324 
325  if (sampling_pose_.orientation_constraint_)
326  v *= sampling_pose_.orientation_constraint_->getXAxisTolerance() *
327  sampling_pose_.orientation_constraint_->getYAxisTolerance() *
328  sampling_pose_.orientation_constraint_->getZAxisTolerance();
329  return v;
330 }
331 
333 {
334  if (sampling_pose_.orientation_constraint_)
335  return sampling_pose_.orientation_constraint_->getLinkModel()->getName();
336  return sampling_pose_.position_constraint_->getLinkModel()->getName();
337 }
338 
340 {
341  if (!kb_)
342  {
343  logError("No IK solver");
344  return false;
345  }
346 
347  // check if we need to transform the request into the coordinate frame expected by IK
348  ik_frame_ = kb_->getBaseFrame();
349  transform_ik_ = !robot_state::Transforms::sameFrame(ik_frame_, jmg_->getParentModel().getModelFrame());
350  if (!ik_frame_.empty() && ik_frame_[0] == '/')
351  ik_frame_.erase(ik_frame_.begin());
352  if (transform_ik_)
353  if (!jmg_->getParentModel().hasLinkModel(ik_frame_))
354  {
355  logError("The IK solver expects requests in frame '%s' but this frame is not known to the sampler. Ignoring "
356  "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  logError("IK cannot be performed for link '%s'. The solver can report IK solutions for link '%s'.",
402  sampling_pose_.position_constraint_ ?
403  sampling_pose_.position_constraint_->getLinkModel()->getName().c_str() :
404  sampling_pose_.orientation_constraint_->getLinkModel()->getName().c_str(),
405  kb_->getTipFrame().c_str());
406  return false;
407  }
408  return true;
409 }
410 
411 bool constraint_samplers::IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat,
412  const robot_state::RobotState& ks, unsigned int max_attempts)
413 {
414  if (ks.dirtyLinkTransforms())
415  {
416  // samplePose below requires accurate transforms
417  logError("IKConstraintSampler received dirty robot state, but valid transforms are required. Failing.");
418  return false;
419  }
420 
421  if (sampling_pose_.position_constraint_)
422  {
423  const std::vector<bodies::BodyPtr>& b = sampling_pose_.position_constraint_->getConstraintRegions();
424  if (!b.empty())
425  {
426  bool found = false;
427  std::size_t k = random_number_generator_.uniformInteger(0, b.size() - 1);
428  for (std::size_t i = 0; i < b.size(); ++i)
429  if (b[(i + k) % b.size()]->samplePointInside(random_number_generator_, max_attempts, pos))
430  {
431  found = true;
432  break;
433  }
434  if (!found)
435  {
436  logError("Unable to sample a point inside the constraint region");
437  return false;
438  }
439  }
440  else
441  {
442  logError("Unable to sample a point inside the constraint region. Constraint region is empty when it should not "
443  "be.");
444  return false;
445  }
446 
447  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
448  // model
449  if (sampling_pose_.position_constraint_->mobileReferenceFrame())
450  pos = ks.getFrameTransform(sampling_pose_.position_constraint_->getReferenceFrame()) * pos;
451  }
452  else
453  {
454  // do FK for rand state
455  robot_state::RobotState tempState(ks);
456  tempState.setToRandomPositions(jmg_);
457  pos = tempState.getGlobalLinkTransform(sampling_pose_.orientation_constraint_->getLinkModel()).translation();
458  }
459 
460  if (sampling_pose_.orientation_constraint_)
461  {
462  // sample a rotation matrix within the allowed bounds
463  double angle_x =
464  2.0 * (random_number_generator_.uniform01() - 0.5) *
465  (sampling_pose_.orientation_constraint_->getXAxisTolerance() - std::numeric_limits<double>::epsilon());
466  double angle_y =
467  2.0 * (random_number_generator_.uniform01() - 0.5) *
468  (sampling_pose_.orientation_constraint_->getYAxisTolerance() - std::numeric_limits<double>::epsilon());
469  double angle_z =
470  2.0 * (random_number_generator_.uniform01() - 0.5) *
471  (sampling_pose_.orientation_constraint_->getZAxisTolerance() - std::numeric_limits<double>::epsilon());
472  Eigen::Affine3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
473  Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
474  Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
475  Eigen::Affine3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
476  quat = Eigen::Quaterniond(reqr.rotation());
477 
478  // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
479  // model
480  if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
481  {
482  const Eigen::Affine3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
483  Eigen::Affine3d rt(t.rotation() * quat.toRotationMatrix());
484  quat = Eigen::Quaterniond(rt.rotation());
485  }
486  }
487  else
488  {
489  // sample a random orientation
490  double q[4];
491  random_number_generator_.quaternion(q);
492  quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
493  }
494 
495  // if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
496  if (sampling_pose_.position_constraint_ && sampling_pose_.position_constraint_->hasLinkOffset())
497  // the rotation matrix that corresponds to the desired orientation
498  pos = pos - quat.toRotationMatrix() * sampling_pose_.position_constraint_->getLinkOffset();
499 
500  return true;
501 }
502 
503 namespace constraint_samplers
504 {
505 namespace
506 {
507 void samplingIkCallbackFnAdapter(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
509  const geometry_msgs::Pose&, const std::vector<double>& ik_sol,
510  moveit_msgs::MoveItErrorCodes& error_code)
511 {
512  const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
513  std::vector<double> solution(bij.size());
514  for (std::size_t i = 0; i < bij.size(); ++i)
515  solution[i] = ik_sol[bij[i]];
516  if (constraint(state, jmg, &solution[0]))
517  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
518  else
520 }
521 }
522 }
523 
525  const robot_state::RobotState& reference_state,
526  unsigned int max_attempts)
527 {
528  return sampleHelper(state, reference_state, max_attempts, false);
529 }
530 
532  const robot_state::RobotState& reference_state,
533  unsigned int max_attempts, bool project)
534 {
535  if (!is_valid_)
536  {
537  logWarn("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  logInform("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() * ikq;
565  point = ikq.translation();
566  quat = Eigen::Quaterniond(ikq.rotation());
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.rotation());
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 
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 
608  const geometry_msgs::Pose& ik_query, 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  logError("IK solver failed with error %d", error.val);
646  else if (verbose_)
647  logInform("IK failed");
648  }
649  return false;
650 }
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
Class for constraints on the orientation of a link.
bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, robot_state::RobotState &state, bool use_as_seed)
Actually calls IK on the given pose, generating a random seed state.
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1308
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 ...
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
bool validate(robot_state::RobotState &state) const
Class for handling single DOF joint constraints.
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double getSamplingVolume() const
Gets the volume associated with the position and orientation constraints.
virtual void clear()
Clears all data from the constraint.
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:59
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
virtual bool sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
Produces an IK sample.
const robot_model::JointModelGroup * jmg_
Holds the joint model group associated with this constraint.
bool loadIKSolver()
Performs checks and sets various internal values associated with the IK solver.
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
virtual void clear()
Clears all data from the constraint.
static const int NO_IK_SOLUTION
Representation and evaluation of kinematic constraints.
virtual bool project(robot_state::RobotState &state, unsigned int max_attempts)
Project a sample given the constraints, updating the joint state group. This function allows the para...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:632
virtual void clear()
Clears all data from the constraint.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
bool sampleHelper(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts, bool project)
void update(bool force=false)
Update all transforms.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
std::vector< JointInfo > bounds_
The bounds for any joint with bounds that are more restrictive than the joint limits.
const std::string & getLinkName() const
Gets the link name associated with this sampler.
bool dirtyLinkTransforms() const
Definition: robot_state.h:1385
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.
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:584
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool is_valid_
Holds the value for validity.
std::vector< const robot_model::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:82
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...
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
Definition: robot_state.h: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.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44