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  , active_variable_count_(0)
109  , is_contiguous_index_list_(true)
110  , is_chain_(false)
111  , is_single_dof_(true)
112  , config_(config)
113 {
114  // sort joints in Depth-First order
115  joint_model_vector_ = unsorted_group_joints;
116  std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
117  joint_model_name_vector_.reserve(joint_model_vector_.size());
118 
119  // figure out active joints, mimic joints, fixed joints
120  // construct index maps, list of variables
121  for (const JointModel* joint_model : joint_model_vector_)
122  {
123  joint_model_name_vector_.push_back(joint_model->getName());
124  joint_model_map_[joint_model->getName()] = joint_model;
125  unsigned int vc = joint_model->getVariableCount();
126  if (vc > 0)
127  {
128  if (vc > 1)
129  is_single_dof_ = false;
130  const std::vector<std::string>& name_order = joint_model->getVariableNames();
131  if (joint_model->getMimic() == nullptr)
132  {
133  active_joint_model_vector_.push_back(joint_model);
134  active_joint_model_name_vector_.push_back(joint_model->getName());
135  active_joint_model_start_index_.push_back(variable_count_);
136  active_joint_models_bounds_.push_back(&joint_model->getVariableBounds());
137  active_variable_count_ += vc;
138  }
139  else
140  mimic_joints_.push_back(joint_model);
141  for (const std::string& name : name_order)
142  {
143  variable_names_.push_back(name);
144  variable_names_set_.insert(name);
145  }
146 
147  int first_index = joint_model->getFirstVariableIndex();
148  for (std::size_t j = 0; j < name_order.size(); ++j)
149  {
150  variable_index_list_.push_back(first_index + j);
151  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
152  }
153  joint_variables_index_map_[joint_model->getName()] = variable_count_;
154 
155  if (joint_model->getType() == JointModel::REVOLUTE &&
156  static_cast<const RevoluteJointModel*>(joint_model)->isContinuous())
157  continuous_joint_model_vector_.push_back(joint_model);
158 
159  variable_count_ += vc;
160  }
161  else
162  fixed_joints_.push_back(joint_model);
163  }
164 
165  // now we need to find all the set of joints within this group
166  // that root distinct subtrees
167  for (const JointModel* active_joint_model : active_joint_model_vector_)
168  {
169  // if we find that an ancestor is also in the group, then the joint is not a root
170  if (!includesParent(active_joint_model, this))
171  joint_roots_.push_back(active_joint_model);
172  }
173 
174  // when updating this group within a state, it is useful to know
175  // if the full state of a group is contiguous within the full state of the robot
176  if (variable_index_list_.empty())
177  is_contiguous_index_list_ = false;
178  else
179  for (std::size_t i = 1; i < variable_index_list_.size(); ++i)
180  if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
181  {
182  is_contiguous_index_list_ = false;
183  break;
184  }
185 
186  // when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
187  for (const JointModel* mimic_joint : mimic_joints_)
188  // if the joint we mimic is also in this group, we will need to do updates when sampling
189  if (hasJointModel(mimic_joint->getMimic()->getName()))
190  {
191  int src = joint_variables_index_map_[mimic_joint->getMimic()->getName()];
192  int dest = joint_variables_index_map_[mimic_joint->getName()];
193  GroupMimicUpdate mu(src, dest, mimic_joint->getMimicFactor(), mimic_joint->getMimicOffset());
194  group_mimic_update_.push_back(mu);
195  }
196 
197  // now we need to make another pass for group links (we include the fixed joints here)
198  std::set<const LinkModel*> group_links_set;
199  for (const JointModel* joint_model : joint_model_vector_)
200  group_links_set.insert(joint_model->getChildLinkModel());
201  for (const LinkModel* group_link : group_links_set)
202  link_model_vector_.push_back(group_link);
203  std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
204 
205  for (const LinkModel* link_model : link_model_vector_)
206  {
207  link_model_map_[link_model->getName()] = link_model;
208  link_model_name_vector_.push_back(link_model->getName());
209  if (!link_model->getShapes().empty())
210  {
211  link_model_with_geometry_vector_.push_back(link_model);
212  link_model_with_geometry_name_vector_.push_back(link_model->getName());
213  }
214  }
215 
216  // compute the common root of this group
217  if (!joint_roots_.empty())
218  {
219  common_root_ = joint_roots_[0];
220  for (std::size_t i = 1; i < joint_roots_.size(); ++i)
221  common_root_ = parent_model->getCommonRoot(joint_roots_[i], common_root_);
222  }
223 
224  // compute updated links
225  for (const JointModel* joint_root : joint_roots_)
226  {
227  const std::vector<const LinkModel*>& links = joint_root->getDescendantLinkModels();
228  updated_link_model_set_.insert(links.begin(), links.end());
229  }
230  for (const LinkModel* updated_link_model : updated_link_model_set_)
231  {
232  updated_link_model_name_set_.insert(updated_link_model->getName());
233  updated_link_model_vector_.push_back(updated_link_model);
234  if (!updated_link_model->getShapes().empty())
235  {
236  updated_link_model_with_geometry_vector_.push_back(updated_link_model);
237  updated_link_model_with_geometry_set_.insert(updated_link_model);
238  updated_link_model_with_geometry_name_set_.insert(updated_link_model->getName());
239  }
240  }
241  std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
242  std::sort(updated_link_model_with_geometry_vector_.begin(), updated_link_model_with_geometry_vector_.end(),
243  OrderLinksByIndex());
244  for (const LinkModel* updated_link_model : updated_link_model_vector_)
245  updated_link_model_name_vector_.push_back(updated_link_model->getName());
246  for (const LinkModel* updated_link_model_with_geometry : updated_link_model_with_geometry_vector_)
247  updated_link_model_with_geometry_name_vector_.push_back(updated_link_model_with_geometry->getName());
248 
249  // check if this group should actually be a chain
250  if (joint_roots_.size() == 1 && !active_joint_model_vector_.empty())
251  {
252  bool chain = true;
253  // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
254  // we should always get to the parent.
255  for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
256  if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
257  {
258  chain = false;
259  break;
260  }
261  if (chain)
262  is_chain_ = true;
263  }
264 }
265 
267 
268 void JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
269 {
270  subgroup_names_ = subgroups;
271  subgroup_names_set_.clear();
272  for (const std::string& subgroup_name : subgroup_names_)
273  subgroup_names_set_.insert(subgroup_name);
274 }
275 
276 void JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
277 {
278  sub_groups.resize(subgroup_names_.size());
279  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
280  sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
281 }
282 
283 bool JointModelGroup::hasJointModel(const std::string& joint) const
284 {
285  return joint_model_map_.find(joint) != joint_model_map_.end();
286 }
287 
288 bool JointModelGroup::hasLinkModel(const std::string& link) const
289 {
290  return link_model_map_.find(link) != link_model_map_.end();
291 }
292 
293 const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const
294 {
295  LinkModelMapConst::const_iterator it = link_model_map_.find(name);
296  if (it == link_model_map_.end())
297  {
298  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
299  return nullptr;
300  }
301  return it->second;
302 }
303 
304 const JointModel* JointModelGroup::getJointModel(const std::string& name) const
305 {
306  JointModelMapConst::const_iterator it = joint_model_map_.find(name);
307  if (it == joint_model_map_.end())
308  {
309  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
310  return nullptr;
311  }
312  return it->second;
313 }
314 
316  const JointBoundsVector& active_joint_bounds) const
317 {
318  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
319  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
321  *active_joint_bounds[i]);
322 
323  updateMimicJoints(values);
324 }
325 
327  const JointBoundsVector& active_joint_bounds, const double* seed,
328  double distance) const
329 {
330  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
331  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
333  *active_joint_bounds[i],
335  distance);
336  updateMimicJoints(values);
337 }
338 
340  const JointBoundsVector& active_joint_bounds, const double* seed,
341  const std::map<JointModel::JointType, double>& distance_map) const
342 {
343  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
344  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
345  {
346  double distance = 0.0;
347  std::map<JointModel::JointType, double>::const_iterator iter =
348  distance_map.find(active_joint_model_vector_[i]->getType());
349  if (iter != distance_map.end())
350  distance = iter->second;
351  else
352  ROS_WARN_NAMED(LOGNAME, "Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str());
353  active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(rng, values + active_joint_model_start_index_[i],
354  *active_joint_bounds[i],
356  distance);
357  }
358  updateMimicJoints(values);
359 }
360 
362  const JointBoundsVector& active_joint_bounds, const double* seed,
363  const std::vector<double>& distances) const
364 {
365  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
366  if (distances.size() != active_joint_model_vector_.size())
367  throw Exception("When sampling random values nearby for group '" + name_ + "', distances vector should be of size " +
368  boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
369  boost::lexical_cast<std::string>(distances.size()));
370  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
372  *active_joint_bounds[i],
374  distances[i]);
375  updateMimicJoints(values);
376 }
377 
378 bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
379  double margin) const
380 {
381  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
382  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
384  *active_joint_bounds[i], margin))
385  return false;
386  return true;
387 }
388 
389 bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
390 {
391  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
392  bool change = false;
393  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
395  *active_joint_bounds[i]))
396  change = true;
397  if (change)
398  updateMimicJoints(state);
399  return change;
400 }
401 
402 double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
403 {
404  double max_distance = 0.0;
405  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
406  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
407  active_joint_model_vector_[j]->getDistanceFactor();
408  return max_distance;
409 }
410 
411 double JointModelGroup::distance(const double* state1, const double* state2) const
412 {
413  double d = 0.0;
414  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
415  d += active_joint_model_vector_[i]->getDistanceFactor() *
417  state2 + active_joint_model_start_index_[i]);
418  return d;
419 }
420 
421 void JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
422 {
423  // we interpolate values only for active joint models (non-mimic)
424  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
428 
429  // now we update mimic as needed
430  updateMimicJoints(state);
431 }
432 
433 void JointModelGroup::updateMimicJoints(double* values) const
434 {
435  // update mimic (only local joints as we are dealing with a local group state)
436  for (const GroupMimicUpdate& mimic_update : group_mimic_update_)
437  values[mimic_update.dest] = values[mimic_update.src] * mimic_update.factor + mimic_update.offset;
438 }
439 
440 void JointModelGroup::addDefaultState(const std::string& name, const std::map<std::string, double>& default_state)
441 {
442  default_states_[name] = default_state;
443  default_states_names_.push_back(name);
444 }
445 
446 bool JointModelGroup::getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const
447 {
448  std::map<std::string, std::map<std::string, double> >::const_iterator it = default_states_.find(name);
449  if (it == default_states_.end())
450  return false;
451  values = it->second;
452  return true;
453 }
454 
456 {
457  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
460 }
461 
462 void JointModelGroup::getVariableDefaultPositions(std::map<std::string, double>& values) const
463 {
464  std::vector<double> tmp(variable_count_);
466  for (std::size_t i = 0; i < variable_names_.size(); ++i)
467  values[variable_names_[i]] = tmp[i];
468 }
469 
470 void JointModelGroup::setEndEffectorName(const std::string& name)
471 {
473 }
474 
475 void JointModelGroup::setEndEffectorParent(const std::string& group, const std::string& link)
476 {
477  end_effector_parent_.first = group;
478  end_effector_parent_.second = link;
479 }
480 
481 void JointModelGroup::attachEndEffector(const std::string& eef_name)
482 {
483  attached_end_effector_names_.push_back(eef_name);
484 }
485 
486 bool JointModelGroup::getEndEffectorTips(std::vector<std::string>& tips) const
487 {
488  // Get a vector of tip links
489  std::vector<const LinkModel*> tip_links;
490  if (!getEndEffectorTips(tip_links))
491  return false;
492 
493  // Convert to string names
494  tips.clear();
495  for (const LinkModel* link_model : tip_links)
496  tips.push_back(link_model->getName());
497  return true;
498 }
499 
500 bool JointModelGroup::getEndEffectorTips(std::vector<const LinkModel*>& tips) const
501 {
502  tips.clear();
503  for (const std::string& name : getAttachedEndEffectorNames())
504  {
506  if (!eef)
507  {
508  ROS_ERROR_NAMED(LOGNAME, "Unable to find joint model group for eef");
509  return false;
510  }
511  const std::string& eef_parent = eef->getEndEffectorParentGroup().second;
512 
513  const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent);
514  if (!eef_link)
515  {
516  ROS_ERROR_NAMED(LOGNAME, "Unable to find end effector link for eef");
517  return false;
518  }
519  // insert eef_link into tips, maintaining a *sorted* vector, thus enabling use of std::lower_bound
520  const auto insert_it = std::lower_bound(tips.cbegin(), tips.cend(), eef_link);
521  if (insert_it == tips.end() || eef_link != *insert_it) // only insert if not a duplicate
522  tips.insert(insert_it, eef_link);
523  }
524  return true;
525 }
526 
527 const LinkModel* JointModelGroup::getOnlyOneEndEffectorTip() const
528 {
529  std::vector<const LinkModel*> tips;
530  getEndEffectorTips(tips);
531  if (tips.size() == 1)
532  return tips.front();
533  else if (tips.size() > 1)
534  ROS_ERROR_NAMED(LOGNAME, "More than one end effector tip found for joint model group, "
535  "so cannot return only one");
536  else
537  ROS_ERROR_NAMED(LOGNAME, "No end effector tips found in joint model group");
538  return nullptr;
539 }
540 
541 int JointModelGroup::getVariableGroupIndex(const std::string& variable) const
542 {
543  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
544  if (it == joint_variables_index_map_.end())
545  {
546  ROS_ERROR_NAMED(LOGNAME, "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str());
547  return -1;
548  }
549  return it->second;
550 }
551 
553 {
554  group_kinematics_.first.default_ik_timeout_ = ik_timeout;
555  if (group_kinematics_.first.solver_instance_)
556  group_kinematics_.first.solver_instance_->setDefaultTimeout(ik_timeout);
557  for (std::pair<const JointModelGroup* const, KinematicsSolver>& it : group_kinematics_.second)
558  it.second.default_ik_timeout_ = ik_timeout;
559 }
560 
561 bool JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
562  std::vector<unsigned int>& joint_bijection) const
563 {
564  joint_bijection.clear();
565  for (const std::string& ik_jname : ik_jnames)
566  {
567  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jname);
568  if (it == joint_variables_index_map_.end())
569  {
570  // skip reported fixed joints
571  if (hasJointModel(ik_jname) && getJointModel(ik_jname)->getType() == JointModel::FIXED)
572  continue;
574  "IK solver computes joint values for joint '%s' "
575  "but group '%s' does not contain such a joint.",
576  ik_jname.c_str(), getName().c_str());
577  return false;
578  }
579  const JointModel* jm = getJointModel(ik_jname);
580  for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
581  joint_bijection.push_back(it->second + k);
582  }
583  return true;
584 }
585 
586 void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
587 {
588  if (solvers.first)
589  {
590  group_kinematics_.first.allocator_ = solvers.first;
591  group_kinematics_.first.solver_instance_ = solvers.first(this);
592  if (group_kinematics_.first.solver_instance_)
593  {
594  group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
595  if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
596  group_kinematics_.first.bijection_))
597  group_kinematics_.first.reset();
598  }
599  }
600  else
601  // we now compute a joint bijection only if we have a solver map
602  for (const std::pair<const JointModelGroup* const, SolverAllocatorFn>& it : solvers.second)
603  if (it.first->getSolverInstance())
604  {
605  KinematicsSolver& ks = group_kinematics_.second[it.first];
606  ks.allocator_ = it.second;
607  ks.solver_instance_ = const_cast<JointModelGroup*>(it.first)->getSolverInstance();
608  ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
609  if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
610  {
611  group_kinematics_.second.clear();
612  break;
613  }
614  }
615 }
616 
617 bool JointModelGroup::canSetStateFromIK(const std::string& tip) const
618 {
619  const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
620  if (!solver || tip.empty())
621  return false;
622 
623  const std::vector<std::string>& tip_frames = solver->getTipFrames();
624 
625  if (tip_frames.empty())
626  {
627  ROS_DEBUG_NAMED(LOGNAME, "Group %s has no tip frame(s)", name_.c_str());
628  return false;
629  }
630 
631  // loop through all tip frames supported by the JMG
632  for (const std::string& tip_frame : tip_frames)
633  {
634  // remove frame reference, if specified
635  const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
636  const std::string& tip_frame_local = tip_frame[0] == '/' ? tip_frame.substr(1) : tip_frame;
637  ROS_DEBUG_NAMED(LOGNAME, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
638  tip_frame_local.c_str());
639 
640  // Check if the IK solver's tip is the same as the frame of inquiry
641  if (tip_local != tip_frame_local)
642  {
643  // If not the same, check if this planning group includes the frame of inquiry
644  if (hasLinkModel(tip_frame_local))
645  {
646  const LinkModel* lm = getLinkModel(tip_frame_local);
647  const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
648  // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
649  for (const std::pair<const LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
650  {
651  if (fixed_link.first->getName() == tip_local)
652  return true;
653  }
654  }
655  }
656  else
657  return true;
658  }
659 
660  // Did not find any valid tip frame links to use
661  return false;
662 }
663 
664 void JointModelGroup::printGroupInfo(std::ostream& out) const
665 {
666  out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
667  out << " * Joints:" << std::endl;
668  for (const JointModel* joint_model : joint_model_vector_)
669  out << " '" << joint_model->getName() << "' (" << joint_model->getTypeName() << ")" << std::endl;
670  out << " * Variables:" << std::endl;
671  for (const std::string& variable_name : variable_names_)
672  {
673  int local_idx = joint_variables_index_map_.find(variable_name)->second;
674  const JointModel* jm = parent_model_->getJointOfVariable(variable_name);
675  out << " '" << variable_name << "', index "
676  << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_name)) << " in full state, index "
677  << local_idx << " in group state";
678  if (jm->getMimic())
679  out << ", mimic '" << jm->getMimic()->getName() << "'";
680  out << std::endl;
681  out << " " << parent_model_->getVariableBounds(variable_name) << std::endl;
682  }
683  out << " * Variables Index List:" << std::endl;
684  out << " ";
685  for (int variable_index : variable_index_list_)
686  out << variable_index << " ";
688  out << "(contiguous)";
689  else
690  out << "(non-contiguous)";
691  out << std::endl;
692  if (group_kinematics_.first)
693  {
694  out << " * Kinematics solver bijection:" << std::endl;
695  out << " ";
696  for (unsigned int index : group_kinematics_.first.bijection_)
697  out << index << " ";
698  out << std::endl;
699  }
700  if (!group_kinematics_.second.empty())
701  {
702  out << " * Compound kinematics solver:" << std::endl;
703  for (const std::pair<const JointModelGroup* const, KinematicsSolver>& it : group_kinematics_.second)
704  {
705  out << " " << it.first->getName() << ":";
706  for (unsigned int index : it.second.bijection_)
707  out << " " << index;
708  out << std::endl;
709  }
710  }
711 
712  if (!group_mimic_update_.empty())
713  {
714  out << " * Local Mimic Updates:" << std::endl;
715  for (const GroupMimicUpdate& mimic_update : group_mimic_update_)
716  out << " [" << mimic_update.dest << "] = " << mimic_update.factor << " * [" << mimic_update.src << "] + "
717  << mimic_update.offset << std::endl;
718  }
719  out << std::endl;
720 }
721 
722 bool JointModelGroup::isValidVelocityMove(const std::vector<double>& from_joint_pose,
723  const std::vector<double>& to_joint_pose, double dt) const
724 {
725  // Check for equal sized arrays
726  if (from_joint_pose.size() != to_joint_pose.size())
727  {
728  ROS_ERROR_NAMED(LOGNAME, "To and from joint poses are of different sizes.");
729  return false;
730  }
731 
732  return isValidVelocityMove(&from_joint_pose[0], &to_joint_pose[0], from_joint_pose.size(), dt);
733 }
734 
735 bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose,
736  std::size_t array_size, double dt) const
737 {
738  const std::vector<const JointModel::Bounds*>& bounds = getActiveJointModelsBounds();
739  const std::vector<unsigned int>& bij = getKinematicsSolverJointBijection();
740 
741  for (std::size_t i = 0; i < array_size; ++i)
742  {
743  double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
744  const std::vector<moveit::core::VariableBounds>* var_bounds = bounds[bij[i]];
745 
746  if (var_bounds->size() != 1)
747  {
748  // TODO(davetcoleman) Support multiple variables
749  ROS_ERROR_NAMED(LOGNAME, "Attempting to check velocity bounds for waypoint move with joints that have multiple "
750  "variables");
751  return false;
752  }
753  const double max_velocity = (*var_bounds)[0].max_velocity_;
754 
755  double max_dtheta = dt * max_velocity;
756  if (dtheta > max_dtheta)
757  {
758  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Not valid velocity move because of joint " << i);
759  return false;
760  }
761  }
762 
763  return true;
764 }
765 } // end of namespace core
766 } // 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:354
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:370
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:506
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:713
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:359
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:584
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:717
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:672
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:499
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:819
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:499
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:788
moveit::core::JointModelGroup::parent_model_
const RobotModel * parent_model_
Owner model.
Definition: joint_model_group.h:660
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:781
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:547
moveit::core::JointModelGroup::canSetStateFromIK
bool canSetStateFromIK(const std::string &tip) const
Definition: joint_model_group.cpp:683
moveit::core::JointModelGroup::printGroupInfo
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
Definition: joint_model_group.cpp:730
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:541
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:553
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:566
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:547
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:768
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:512
moveit::core::JointModelGroup::link_model_map_
LinkModelMapConst link_model_map_
A map from link names to their instances.
Definition: joint_model_group.h:725
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:789
name
std::string name
moveit::core::JointModelGroup::name_
std::string name_
Name of group.
Definition: joint_model_group.h:663
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:487
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:816
moveit::core::JointModelGroup::group_mimic_update_
std::vector< GroupMimicUpdate > group_mimic_update_
Definition: joint_model_group.h:809
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:792
moveit::core::RobotModel::getJointOfVariable
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:279
moveit::core::JointModelGroup::distance
double distance(const double *state1, const double *state2) const
Definition: joint_model_group.cpp:477
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
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:811
moveit::core::JointModelGroup::getVariableRandomPositionsNearBy
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *seed, const double distance) const
Compute random values for the state of the joint group.
Definition: joint_model_group.h:397
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:784
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:342
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:349
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:706
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:536
moveit::core::JointModelGroup::setSolverAllocators
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
Definition: joint_model_group.h:594
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:1214
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:334
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:602
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:778
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:618
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:666
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:695
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:593
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:634
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:688
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:775
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:547
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:627
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:607
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:514


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21