joint_model_group.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan, Dave Coleman */
37 
42 #include <boost/lexical_cast.hpp>
43 #include <algorithm>
44 #include "order_robot_model_items.inc"
45 
46 namespace moveit
47 {
48 namespace core
49 {
50 namespace
51 {
52 // check if a parent or ancestor of joint is included in this group
53 bool includesParent(const JointModel* joint, const JointModelGroup* group)
54 {
55  bool found = false;
56  // if we find that an ancestor is also in the group, then the joint is not a root
57  while (joint->getParentLinkModel() != nullptr)
58  {
59  joint = joint->getParentLinkModel()->getParentJointModel();
60  if (group->hasJointModel(joint->getName()) && joint->getVariableCount() > 0 && joint->getMimic() == nullptr)
61  {
62  found = true;
63  break;
64  }
65  else if (joint->getMimic() != nullptr)
66  {
67  const JointModel* mjoint = joint->getMimic();
68  if (group->hasJointModel(mjoint->getName()) && mjoint->getVariableCount() > 0 && mjoint->getMimic() == nullptr)
69  found = true;
70  else if (includesParent(mjoint, group))
71  found = true;
72  if (found)
73  break;
74  }
75  }
76  return found;
77 }
78 
79 // check if joint a is right below b, in the kinematic chain, with no active DOF missing
80 bool jointPrecedes(const JointModel* a, const JointModel* b)
81 {
82  if (!a->getParentLinkModel())
83  return false;
84  const JointModel* p = a->getParentLinkModel()->getParentJointModel();
85  while (p)
86  {
87  if (p == b)
88  return true;
89  if (p->getType() == JointModel::FIXED)
90  p = p->getParentLinkModel() ? p->getParentLinkModel()->getParentJointModel() : nullptr;
91  else
92  break;
93  }
94 
95  return false;
96 }
97 } // namespace
98 
99 const std::string LOGNAME = "robot_model.jmg";
100 
101 JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config,
102  const std::vector<const JointModel*>& unsorted_group_joints,
103  const RobotModel* parent_model)
104  : parent_model_(parent_model)
105  , name_(group_name)
106  , common_root_(nullptr)
107  , variable_count_(0)
108  , is_contiguous_index_list_(true)
109  , is_chain_(false)
110  , is_single_dof_(true)
111  , config_(config)
112 {
113  // sort joints in Depth-First order
114  joint_model_vector_ = unsorted_group_joints;
115  std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
116  joint_model_name_vector_.reserve(joint_model_vector_.size());
117 
118  // figure out active joints, mimic joints, fixed joints
119  // construct index maps, list of variables
120  for (const JointModel* joint_model : joint_model_vector_)
121  {
122  joint_model_name_vector_.push_back(joint_model->getName());
123  joint_model_map_[joint_model->getName()] = joint_model;
124  unsigned int vc = joint_model->getVariableCount();
125  if (vc > 0)
126  {
127  if (vc > 1)
128  is_single_dof_ = false;
129  const std::vector<std::string>& name_order = joint_model->getVariableNames();
130  if (joint_model->getMimic() == nullptr)
131  {
132  active_joint_model_vector_.push_back(joint_model);
133  active_joint_model_name_vector_.push_back(joint_model->getName());
134  active_joint_model_start_index_.push_back(variable_count_);
135  active_joint_models_bounds_.push_back(&joint_model->getVariableBounds());
136  }
137  else
138  mimic_joints_.push_back(joint_model);
139  for (const std::string& name : name_order)
140  {
141  variable_names_.push_back(name);
142  variable_names_set_.insert(name);
143  }
144 
145  int first_index = joint_model->getFirstVariableIndex();
146  for (std::size_t j = 0; j < name_order.size(); ++j)
147  {
148  variable_index_list_.push_back(first_index + j);
149  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
150  }
151  joint_variables_index_map_[joint_model->getName()] = variable_count_;
152 
153  if (joint_model->getType() == JointModel::REVOLUTE &&
154  static_cast<const RevoluteJointModel*>(joint_model)->isContinuous())
155  continuous_joint_model_vector_.push_back(joint_model);
156 
157  variable_count_ += vc;
158  }
159  else
160  fixed_joints_.push_back(joint_model);
161  }
162 
163  // now we need to find all the set of joints within this group
164  // that root distinct subtrees
165  for (const JointModel* active_joint_model : active_joint_model_vector_)
166  {
167  // if we find that an ancestor is also in the group, then the joint is not a root
168  if (!includesParent(active_joint_model, this))
169  joint_roots_.push_back(active_joint_model);
170  }
171 
172  // when updating this group within a state, it is useful to know
173  // if the full state of a group is contiguous within the full state of the robot
174  if (variable_index_list_.empty())
175  is_contiguous_index_list_ = false;
176  else
177  for (std::size_t i = 1; i < variable_index_list_.size(); ++i)
178  if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
179  {
180  is_contiguous_index_list_ = false;
181  break;
182  }
183 
184  // when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
185  for (const JointModel* mimic_joint : mimic_joints_)
186  // if the joint we mimic is also in this group, we will need to do updates when sampling
187  if (hasJointModel(mimic_joint->getMimic()->getName()))
188  {
189  int src = joint_variables_index_map_[mimic_joint->getMimic()->getName()];
190  int dest = joint_variables_index_map_[mimic_joint->getName()];
191  GroupMimicUpdate mu(src, dest, mimic_joint->getMimicFactor(), mimic_joint->getMimicOffset());
192  group_mimic_update_.push_back(mu);
193  }
194 
195  // now we need to make another pass for group links (we include the fixed joints here)
196  std::set<const LinkModel*> group_links_set;
197  for (const JointModel* joint_model : joint_model_vector_)
198  group_links_set.insert(joint_model->getChildLinkModel());
199  for (const LinkModel* group_link : group_links_set)
200  link_model_vector_.push_back(group_link);
201  std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
202 
203  for (const LinkModel* link_model : link_model_vector_)
204  {
205  link_model_map_[link_model->getName()] = link_model;
206  link_model_name_vector_.push_back(link_model->getName());
207  if (!link_model->getShapes().empty())
208  {
209  link_model_with_geometry_vector_.push_back(link_model);
210  link_model_with_geometry_name_vector_.push_back(link_model->getName());
211  }
212  }
213 
214  // compute the common root of this group
215  if (!joint_roots_.empty())
216  {
217  common_root_ = joint_roots_[0];
218  for (std::size_t i = 1; i < joint_roots_.size(); ++i)
219  common_root_ = parent_model->getCommonRoot(joint_roots_[i], common_root_);
220  }
221 
222  // compute updated links
223  for (const JointModel* joint_root : joint_roots_)
224  {
225  const std::vector<const LinkModel*>& links = joint_root->getDescendantLinkModels();
226  updated_link_model_set_.insert(links.begin(), links.end());
227  }
228  for (const LinkModel* updated_link_model : updated_link_model_set_)
229  {
230  updated_link_model_name_set_.insert(updated_link_model->getName());
231  updated_link_model_vector_.push_back(updated_link_model);
232  if (!updated_link_model->getShapes().empty())
233  {
234  updated_link_model_with_geometry_vector_.push_back(updated_link_model);
235  updated_link_model_with_geometry_set_.insert(updated_link_model);
236  updated_link_model_with_geometry_name_set_.insert(updated_link_model->getName());
237  }
238  }
239  std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
240  std::sort(updated_link_model_with_geometry_vector_.begin(), updated_link_model_with_geometry_vector_.end(),
241  OrderLinksByIndex());
242  for (const LinkModel* updated_link_model : updated_link_model_vector_)
243  updated_link_model_name_vector_.push_back(updated_link_model->getName());
244  for (const LinkModel* updated_link_model_with_geometry : updated_link_model_with_geometry_vector_)
245  updated_link_model_with_geometry_name_vector_.push_back(updated_link_model_with_geometry->getName());
246 
247  // check if this group should actually be a chain
248  if (joint_roots_.size() == 1 && !active_joint_model_vector_.empty())
249  {
250  bool chain = true;
251  // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
252  // we should always get to the parent.
253  for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
254  if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
255  {
256  chain = false;
257  break;
258  }
259  if (chain)
260  is_chain_ = true;
261  }
262 }
263 
265 
266 void JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
267 {
268  subgroup_names_ = subgroups;
269  subgroup_names_set_.clear();
270  for (const std::string& subgroup_name : subgroup_names_)
271  subgroup_names_set_.insert(subgroup_name);
272 }
273 
274 void JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
275 {
276  sub_groups.resize(subgroup_names_.size());
277  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
278  sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
279 }
280 
281 bool JointModelGroup::hasJointModel(const std::string& joint) const
282 {
283  return joint_model_map_.find(joint) != joint_model_map_.end();
284 }
285 
286 bool JointModelGroup::hasLinkModel(const std::string& link) const
287 {
288  return link_model_map_.find(link) != link_model_map_.end();
289 }
290 
291 const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const
292 {
293  LinkModelMapConst::const_iterator it = link_model_map_.find(name);
294  if (it == link_model_map_.end())
295  {
296  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
297  return nullptr;
298  }
299  return it->second;
300 }
301 
302 const JointModel* JointModelGroup::getJointModel(const std::string& name) const
303 {
304  JointModelMapConst::const_iterator it = joint_model_map_.find(name);
305  if (it == joint_model_map_.end())
306  {
307  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
308  return nullptr;
309  }
310  return it->second;
311 }
312 
314  const JointBoundsVector& active_joint_bounds) const
315 {
316  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
317  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
319  *active_joint_bounds[i]);
320 
321  updateMimicJoints(values);
322 }
323 
325  const JointBoundsVector& active_joint_bounds, const double* near,
326  double distance) const
327 {
328  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
329  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
331  *active_joint_bounds[i],
333  distance);
334  updateMimicJoints(values);
335 }
336 
338  const JointBoundsVector& active_joint_bounds, const double* near,
339  const std::map<JointModel::JointType, double>& distance_map) const
340 {
341  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
342  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
343  {
344  double distance = 0.0;
345  std::map<JointModel::JointType, double>::const_iterator iter =
346  distance_map.find(active_joint_model_vector_[i]->getType());
347  if (iter != distance_map.end())
348  distance = iter->second;
349  else
350  ROS_WARN_NAMED(LOGNAME, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str());
351  active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i],
352  *active_joint_bounds[i],
354  distance);
355  }
356  updateMimicJoints(values);
357 }
358 
360  const JointBoundsVector& active_joint_bounds, const double* near,
361  const std::vector<double>& distances) const
362 {
363  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
364  if (distances.size() != active_joint_model_vector_.size())
365  throw Exception("When sampling random values nearby for group '" + name_ + "', distances vector should be of size " +
366  boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
367  boost::lexical_cast<std::string>(distances.size()));
368  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
370  *active_joint_bounds[i],
372  distances[i]);
373  updateMimicJoints(values);
374 }
375 
376 bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
377  double margin) const
378 {
379  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
380  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
382  *active_joint_bounds[i], margin))
383  return false;
384  return true;
385 }
386 
387 bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
388 {
389  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
390  bool change = false;
391  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
393  *active_joint_bounds[i]))
394  change = true;
395  if (change)
396  updateMimicJoints(state);
397  return change;
398 }
399 
400 double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
401 {
402  double max_distance = 0.0;
403  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
404  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
405  active_joint_model_vector_[j]->getDistanceFactor();
406  return max_distance;
407 }
408 
409 double JointModelGroup::distance(const double* state1, const double* state2) const
410 {
411  double d = 0.0;
412  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
413  d += active_joint_model_vector_[i]->getDistanceFactor() *
415  state2 + active_joint_model_start_index_[i]);
416  return d;
417 }
418 
419 void JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
420 {
421  // we interpolate values only for active joint models (non-mimic)
422  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
426 
427  // now we update mimic as needed
428  updateMimicJoints(state);
429 }
430 
431 void JointModelGroup::updateMimicJoints(double* values) const
432 {
433  // update mimic (only local joints as we are dealing with a local group state)
434  for (const GroupMimicUpdate& mimic_update : group_mimic_update_)
435  values[mimic_update.dest] = values[mimic_update.src] * mimic_update.factor + mimic_update.offset;
436 }
437 
438 void JointModelGroup::addDefaultState(const std::string& name, const std::map<std::string, double>& default_state)
439 {
440  default_states_[name] = default_state;
441  default_states_names_.push_back(name);
442 }
443 
444 bool JointModelGroup::getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const
445 {
446  std::map<std::string, std::map<std::string, double> >::const_iterator it = default_states_.find(name);
447  if (it == default_states_.end())
448  return false;
449  values = it->second;
450  return true;
451 }
452 
454 {
455  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
458 }
459 
460 void JointModelGroup::getVariableDefaultPositions(std::map<std::string, double>& values) const
461 {
462  std::vector<double> tmp(variable_count_);
464  for (std::size_t i = 0; i < variable_names_.size(); ++i)
465  values[variable_names_[i]] = tmp[i];
466 }
467 
468 void JointModelGroup::setEndEffectorName(const std::string& name)
469 {
471 }
472 
473 void JointModelGroup::setEndEffectorParent(const std::string& group, const std::string& link)
474 {
475  end_effector_parent_.first = group;
476  end_effector_parent_.second = link;
477 }
478 
479 void JointModelGroup::attachEndEffector(const std::string& eef_name)
480 {
481  attached_end_effector_names_.push_back(eef_name);
482 }
483 
484 bool JointModelGroup::getEndEffectorTips(std::vector<std::string>& tips) const
485 {
486  // Get a vector of tip links
487  std::vector<const LinkModel*> tip_links;
488  if (!getEndEffectorTips(tip_links))
489  return false;
490 
491  // Convert to string names
492  tips.clear();
493  for (const LinkModel* link_model : tip_links)
494  tips.push_back(link_model->getName());
495  return true;
496 }
497 
498 bool JointModelGroup::getEndEffectorTips(std::vector<const LinkModel*>& tips) const
499 {
500  tips.clear();
501  for (const std::string& name : getAttachedEndEffectorNames())
502  {
504  if (!eef)
505  {
506  ROS_ERROR_NAMED(LOGNAME, "Unable to find joint model group for eef");
507  return false;
508  }
509  const std::string& eef_parent = eef->getEndEffectorParentGroup().second;
510 
511  const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent);
512  if (!eef_link)
513  {
514  ROS_ERROR_NAMED(LOGNAME, "Unable to find end effector link for eef");
515  return false;
516  }
517  // insert eef_link into tips, maintaining a *sorted* vector, thus enabling use of std::lower_bound
518  const auto insert_it = std::lower_bound(tips.cbegin(), tips.cend(), eef_link);
519  if (insert_it == tips.end() || eef_link != *insert_it) // only insert if not a duplicate
520  tips.insert(insert_it, eef_link);
521  }
522  return true;
523 }
524 
525 const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const
526 {
527  std::vector<const LinkModel*> tips;
528  getEndEffectorTips(tips);
529  if (tips.size() == 1)
530  return tips.front();
531  else if (tips.size() > 1)
532  ROS_ERROR_NAMED(LOGNAME, "More than one end effector tip found for joint model group, "
533  "so cannot return only one");
534  else
535  ROS_ERROR_NAMED(LOGNAME, "No end effector tips found in joint model group");
536  return nullptr;
537 }
538 
539 int JointModelGroup::getVariableGroupIndex(const std::string& variable) const
540 {
541  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
542  if (it == joint_variables_index_map_.end())
543  {
544  ROS_ERROR_NAMED(LOGNAME, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str());
545  return -1;
546  }
547  return it->second;
548 }
549 
551 {
552  group_kinematics_.first.default_ik_timeout_ = ik_timeout;
553  if (group_kinematics_.first.solver_instance_)
554  group_kinematics_.first.solver_instance_->setDefaultTimeout(ik_timeout);
555  for (std::pair<const JointModelGroup* const, KinematicsSolver>& it : group_kinematics_.second)
556  it.second.default_ik_timeout_ = ik_timeout;
557 }
558 
559 bool JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
560  std::vector<unsigned int>& joint_bijection) const
561 {
562  joint_bijection.clear();
563  for (const std::string& ik_jname : ik_jnames)
564  {
565  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jname);
566  if (it == joint_variables_index_map_.end())
567  {
568  // skip reported fixed joints
569  if (hasJointModel(ik_jname) && getJointModel(ik_jname)->getType() == JointModel::FIXED)
570  continue;
572  "IK solver computes joint values for joint '%s' "
573  "but group '%s' does not contain such a joint.",
574  ik_jname.c_str(), getName().c_str());
575  return false;
576  }
577  const JointModel* jm = getJointModel(ik_jname);
578  for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
579  joint_bijection.push_back(it->second + k);
580  }
581  return true;
582 }
583 
584 void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
585 {
586  if (solvers.first)
587  {
588  group_kinematics_.first.allocator_ = solvers.first;
589  group_kinematics_.first.solver_instance_ = solvers.first(this);
590  if (group_kinematics_.first.solver_instance_)
591  {
592  group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
593  if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
594  group_kinematics_.first.bijection_))
595  group_kinematics_.first.reset();
596  }
597  }
598  else
599  // we now compute a joint bijection only if we have a solver map
600  for (const std::pair<const JointModelGroup* const, SolverAllocatorFn>& it : solvers.second)
601  if (it.first->getSolverInstance())
602  {
603  KinematicsSolver& ks = group_kinematics_.second[it.first];
604  ks.allocator_ = it.second;
605  ks.solver_instance_ = const_cast<JointModelGroup*>(it.first)->getSolverInstance();
606  ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
607  if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
608  {
609  group_kinematics_.second.clear();
610  break;
611  }
612  }
613 }
614 
615 bool JointModelGroup::canSetStateFromIK(const std::string& tip) const
616 {
617  const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
618  if (!solver || tip.empty())
619  return false;
620 
621  const std::vector<std::string>& tip_frames = solver->getTipFrames();
622 
623  if (tip_frames.empty())
624  {
625  ROS_DEBUG_NAMED(LOGNAME, "Group %s has no tip frame(s)", name_.c_str());
626  return false;
627  }
628 
629  // loop through all tip frames supported by the JMG
630  for (const std::string& tip_frame : tip_frames)
631  {
632  // remove frame reference, if specified
633  const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
634  const std::string& tip_frame_local = tip_frame[0] == '/' ? tip_frame.substr(1) : tip_frame;
635  ROS_DEBUG_NAMED(LOGNAME, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
636  tip_frame_local.c_str());
637 
638  // Check if the IK solver's tip is the same as the frame of inquiry
639  if (tip_local != tip_frame_local)
640  {
641  // If not the same, check if this planning group includes the frame of inquiry
642  if (hasLinkModel(tip_frame_local))
643  {
644  const LinkModel* lm = getLinkModel(tip_frame_local);
645  const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
646  // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
647  for (const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
648  {
649  if (fixed_link.first->getName() == tip_local)
650  return true;
651  }
652  }
653  }
654  else
655  return true;
656  }
657 
658  // Did not find any valid tip frame links to use
659  return false;
660 }
661 
662 void JointModelGroup::printGroupInfo(std::ostream& out) const
663 {
664  out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
665  out << " * Joints:" << std::endl;
666  for (const JointModel* joint_model : joint_model_vector_)
667  out << " '" << joint_model->getName() << "' (" << joint_model->getTypeName() << ")" << std::endl;
668  out << " * Variables:" << std::endl;
669  for (const std::string& variable_name : variable_names_)
670  {
671  int local_idx = joint_variables_index_map_.find(variable_name)->second;
672  const JointModel* jm = parent_model_->getJointOfVariable(variable_name);
673  out << " '" << variable_name << "', index "
674  << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_name)) << " in full state, index "
675  << local_idx << " in group state";
676  if (jm->getMimic())
677  out << ", mimic '" << jm->getMimic()->getName() << "'";
678  out << std::endl;
679  out << " " << parent_model_->getVariableBounds(variable_name) << std::endl;
680  }
681  out << " * Variables Index List:" << std::endl;
682  out << " ";
683  for (int variable_index : variable_index_list_)
684  out << variable_index << " ";
686  out << "(contiguous)";
687  else
688  out << "(non-contiguous)";
689  out << std::endl;
690  if (group_kinematics_.first)
691  {
692  out << " * Kinematics solver bijection:" << std::endl;
693  out << " ";
694  for (unsigned int index : group_kinematics_.first.bijection_)
695  out << index << " ";
696  out << std::endl;
697  }
698  if (!group_kinematics_.second.empty())
699  {
700  out << " * Compound kinematics solver:" << std::endl;
701  for (const std::pair<const JointModelGroup* const, KinematicsSolver>& it : group_kinematics_.second)
702  {
703  out << " " << it.first->getName() << ":";
704  for (unsigned int index : it.second.bijection_)
705  out << " " << index;
706  out << std::endl;
707  }
708  }
709 
710  if (!group_mimic_update_.empty())
711  {
712  out << " * Local Mimic Updates:" << std::endl;
713  for (const GroupMimicUpdate& mimic_update : group_mimic_update_)
714  out << " [" << mimic_update.dest << "] = " << mimic_update.factor << " * [" << mimic_update.src << "] + "
715  << mimic_update.offset << std::endl;
716  }
717  out << std::endl;
718 }
719 
720 bool JointModelGroup::isValidVelocityMove(const std::vector<double>& from_joint_pose,
721  const std::vector<double>& to_joint_pose, double dt) const
722 {
723  // Check for equal sized arrays
724  if (from_joint_pose.size() != to_joint_pose.size())
725  {
726  ROS_ERROR_NAMED(LOGNAME, "To and from joint poses are of different sizes.");
727  return false;
728  }
729 
730  return isValidVelocityMove(&from_joint_pose[0], &to_joint_pose[0], from_joint_pose.size(), dt);
731 }
732 
733 bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose,
734  std::size_t array_size, double dt) const
735 {
736  const std::vector<const JointModel::Bounds*>& bounds = getActiveJointModelsBounds();
737  const std::vector<unsigned int>& bij = getKinematicsSolverJointBijection();
738 
739  for (std::size_t i = 0; i < array_size; ++i)
740  {
741  double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
742  const std::vector<moveit::core::VariableBounds>* var_bounds = bounds[bij[i]];
743 
744  if (var_bounds->size() != 1)
745  {
746  // TODO(davetcoleman) Support multiple variables
747  ROS_ERROR_NAMED(LOGNAME, "Attempting to check velocity bounds for waypoint move with joints that have multiple "
748  "variables");
749  return false;
750  }
751  const double max_velocity = (*var_bounds)[0].max_velocity_;
752 
753  double max_dtheta = dt * max_velocity;
754  if (dtheta > max_dtheta)
755  {
756  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Not valid velocity move because of joint " << i);
757  return false;
758  }
759  }
760 
761  return true;
762 }
763 } // end of namespace core
764 } // end of namespace moveit
moveit::core::JointModelGroup::hasLinkModel
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
Definition: joint_model_group.cpp:352
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core::JointModelGroup::getJointModel
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
Definition: joint_model_group.cpp:368
exceptions.h
moveit::core::JointModel::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:261
moveit::core::JointModelGroup::addDefaultState
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
Definition: joint_model_group.cpp:504
name_
std::string name_
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::JointModelGroup::variable_index_list_
std::vector< int > variable_index_list_
The list of index values this group includes, with respect to a full robot state; this includes mimic...
Definition: joint_model_group.h:706
moveit::core::JointModelGroup::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
Definition: joint_model_group.cpp:357
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
moveit::core::JointModelGroup::getActiveJointModelsBounds
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
Definition: joint_model_group.h:577
moveit::core::JointModelGroup::active_joint_model_start_index_
std::vector< int > active_joint_model_start_index_
For each active joint model in this group, hold the index at which the corresponding joint state star...
Definition: joint_model_group.h:710
moveit::core::JointModelGroup::active_joint_model_vector_
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
Definition: joint_model_group.h:665
moveit::core::JointModel::getName
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:196
moveit::core::RobotModel::getVariableBounds
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
Definition: robot_model.h:474
moveit::core::JointModelGroup::default_states_names_
std::vector< std::string > default_states_names_
The names of the default states specified for this group in the SRDF.
Definition: joint_model_group.h:809
moveit::core::JointModelGroup::updateMimicJoints
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints....
Definition: joint_model_group.cpp:497
moveit::core::JointModelGroup::isValidVelocityMove
bool isValidVelocityMove(const std::vector< double > &from_joint_pose, const std::vector< double > &to_joint_pose, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits.
Definition: joint_model_group.cpp:786
moveit::core::JointModelGroup::parent_model_
const RobotModel * parent_model_
Owner model.
Definition: joint_model_group.h:653
moveit::core::JointBoundsVector
std::vector< const JointModel::Bounds * > JointBoundsVector
Definition: joint_model_group.h:132
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::JointModelGroup::subgroup_names_set_
std::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
Definition: joint_model_group.h:771
moveit::core::RobotModel::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
Definition: robot_model.cpp:543
moveit::core::JointModelGroup::canSetStateFromIK
bool canSetStateFromIK(const std::string &tip) const
Definition: joint_model_group.cpp:681
moveit::core::JointModelGroup::printGroupInfo
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
Definition: joint_model_group.cpp:728
moveit::core::JointModelGroup::setEndEffectorParent
void setEndEffectorParent(const std::string &group, const std::string &link)
If this group is an end-effector, specify the parent group (e.g., the arm holding the eef) and the li...
Definition: joint_model_group.cpp:539
robot_model.h
moveit::core::JointModelGroup::getAttachedEndEffectorNames
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
Definition: joint_model_group.h:546
moveit::core::JointModel::REVOLUTE
@ REVOLUTE
Definition: joint_model.h:180
moveit::core::JointModelGroup::getEndEffectorTips
bool getEndEffectorTips(std::vector< const LinkModel * > &tips) const
Get the unique set of end effector tips included in a particular joint model group as defined by the ...
Definition: joint_model_group.cpp:564
moveit::core::JointModelGroup::attachEndEffector
void attachEndEffector(const std::string &eef_name)
Notify this group that there is an end-effector attached to it.
Definition: joint_model_group.cpp:545
moveit::core::JointModelGroup::getName
const std::string & getName() const
Get the name of the joint group.
Definition: joint_model_group.h:185
moveit::core::JointModelGroup::variable_count_
unsigned int variable_count_
The number of variables necessary to describe this group of joints.
Definition: joint_model_group.h:761
moveit::core::JointModelGroup::getVariableDefaultPositions
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure.
Definition: joint_model_group.cpp:510
moveit::core::JointModelGroup::link_model_map_
LinkModelMapConst link_model_map_
A map from link names to their instances.
Definition: joint_model_group.h:718
moveit::core::JointModelGroup::end_effector_parent_
std::pair< std::string, std::string > end_effector_parent_
First: name of the group that is parent to this end-effector group; Second: the link this in the pare...
Definition: joint_model_group.h:779
name
std::string name
moveit::core::JointModelGroup::name_
std::string name_
Name of group.
Definition: joint_model_group.h:656
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
moveit::core::JointModelGroup::interpolate
void interpolate(const double *from, const double *to, double t, double *state) const
Definition: joint_model_group.cpp:485
moveit::core::JointModelGroup::default_states_
std::map< std::string, std::map< std::string, double > > default_states_
The set of default states specified for this group in the SRDF.
Definition: joint_model_group.h:806
moveit::core::JointModelGroup::group_mimic_update_
std::vector< GroupMimicUpdate > group_mimic_update_
Definition: joint_model_group.h:799
moveit::core::JointModelGroup::end_effector_name_
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
Definition: joint_model_group.h:782
moveit::core::RobotModel::getJointOfVariable
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:264
moveit::core::JointModelGroup::distance
double distance(const double *state1, const double *state2) const
Definition: joint_model_group.cpp:475
moveit::core::LinkModel::getAssociatedFixedTransforms
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms. The returned transforms are ...
Definition: link_model.h:198
moveit::core::JointModelGroup::getVariableRandomPositionsNearBy
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const double distance) const
Compute random values for the state of the joint group.
Definition: joint_model_group.h:397
d
d
moveit::core::JointModel::variable_names_
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
Definition: joint_model.h:535
moveit::core::JointModelGroup::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
Definition: joint_model_group.h:384
moveit::core::JointModelGroup::group_kinematics_
std::pair< KinematicsSolver, KinematicsSolverMap > group_kinematics_
Definition: joint_model_group.h:801
moveit::core::LOGNAME
const std::string LOGNAME
Definition: joint_model_group.cpp:165
moveit::core::LinkTransformMap
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:68
moveit::core::JointModelGroup::attached_end_effector_names_
std::vector< std::string > attached_end_effector_names_
If an end-effector is attached to this group, the name of that end-effector is stored in this variabl...
Definition: joint_model_group.h:774
moveit::core::JointModelGroup::getSubgroups
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
Definition: joint_model_group.cpp:340
moveit::core::JointModelGroup::hasJointModel
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
Definition: joint_model_group.cpp:347
revolute_joint_model.h
random_numbers::RandomNumberGenerator
moveit::core::JointModelGroup::satisfiesPositionBounds
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: joint_model_group.h:457
moveit::core::JointModelGroup::JointModelGroup
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel * > &joint_vector, const RobotModel *parent_model)
Definition: joint_model_group.cpp:167
moveit::core::JointModelGroup::joint_variables_index_map_
VariableIndexMap joint_variables_index_map_
The group includes all the joint variables that make up the joints the group consists of....
Definition: joint_model_group.h:699
moveit::core::JointModelGroup::setEndEffectorName
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
Definition: joint_model_group.cpp:534
moveit::core::JointModelGroup::setSolverAllocators
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
Definition: joint_model_group.h:587
moveit::core::RobotModel::getLinkModel
const LinkModel * getLinkModel(const std::string &link, bool *has_link=nullptr) const
Get a link by its name. Output error and return NULL when the link is missing.
Definition: robot_model.cpp:1204
moveit::core::JointModelGroup::~JointModelGroup
~JointModelGroup()
moveit::core::JointModelGroup::setSubgroupNames
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
Definition: joint_model_group.cpp:332
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
moveit::core::JointModel::JointModel
JointModel(const std::string &name)
Construct a joint named name.
Definition: joint_model.cpp:113
moveit::core::JointModel::getMimic
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:435
moveit::core::JointModelGroup::getSolverInstance
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
Definition: joint_model_group.h:595
moveit::core::JointModelGroup::subgroup_names_
std::vector< std::string > subgroup_names_
The set of labelled subgroups that are included in this group.
Definition: joint_model_group.h:768
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
moveit::core::JointModelGroup::setDefaultIKTimeout
void setDefaultIKTimeout(double ik_timeout)
Set the default IK timeout.
Definition: joint_model_group.cpp:616
index
unsigned int index
moveit::core::JointModelGroup::joint_model_vector_
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
Definition: joint_model_group.h:659
moveit::core::JointModelGroup::joint_model_map_
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group.
Definition: joint_model_group.h:688
moveit::core::JointModelGroup::getOnlyOneEndEffectorTip
const moveit::core::LinkModel * getOnlyOneEndEffectorTip() const
Get one end effector tip, throwing an error if there ends up being more in the joint model group This...
Definition: joint_model_group.cpp:591
moveit::core::JointModelGroup::getKinematicsSolverJointBijection
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
Definition: joint_model_group.h:627
joint_model_group.h
srdf::Model::Group
moveit::core::JointModelGroup::variable_names_
std::vector< std::string > variable_names_
The names of the DOF that make up this group (this is just a sequence of joint variable names; not ne...
Definition: joint_model_group.h:681
moveit::Exception
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:84
moveit::core::JointModelGroup::is_contiguous_index_list_
bool is_contiguous_index_list_
True if the state of this group is contiguous within the full robot state; this also means that the i...
Definition: joint_model_group.h:765
moveit::core::JointModelGroup::enforcePositionBounds
bool enforcePositionBounds(double *state) const
Definition: joint_model_group.h:451
moveit::core::JointModelGroup::getEndEffectorParentGroup
const std::pair< std::string, std::string > & getEndEffectorParentGroup() const
Get the name of the group this end-effector attaches to (first) and the name of the link in that grou...
Definition: joint_model_group.h:540
moveit::core::JointModel::FIXED
@ FIXED
Definition: joint_model.h:184
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
moveit::core::JointModelGroup::computeIKIndexBijection
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< unsigned int > &joint_bijection) const
Definition: joint_model_group.cpp:625
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
moveit::core::JointModelGroup::getVariableGroupIndex
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
Definition: joint_model_group.cpp:605
moveit::core::JointModelGroup::getMaximumExtent
double getMaximumExtent() const
Definition: joint_model_group.h:464
moveit::core::RobotModel::getEndEffector
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
Definition: robot_model.cpp:510


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:44