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 }
98 
99 JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config,
100  const std::vector<const JointModel*>& unsorted_group_joints,
101  const RobotModel* parent_model)
102  : parent_model_(parent_model)
103  , name_(group_name)
104  , common_root_(nullptr)
105  , variable_count_(0)
106  , is_contiguous_index_list_(true)
107  , is_chain_(false)
108  , is_single_dof_(true)
109  , config_(config)
110 {
111  // sort joints in Depth-First order
112  joint_model_vector_ = unsorted_group_joints;
113  std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
115 
116  // figure out active joints, mimic joints, fixed joints
117  // construct index maps, list of variables
118  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
119  {
121  joint_model_map_[joint_model_vector_[i]->getName()] = joint_model_vector_[i];
122  unsigned int vc = joint_model_vector_[i]->getVariableCount();
123  if (vc > 0)
124  {
125  if (vc > 1)
126  is_single_dof_ = false;
127  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
128  if (joint_model_vector_[i]->getMimic() == nullptr)
129  {
130  active_joint_model_vector_.push_back(joint_model_vector_[i]);
131  active_joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
133  active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
134  }
135  else
136  mimic_joints_.push_back(joint_model_vector_[i]);
137  for (std::size_t j = 0; j < name_order.size(); ++j)
138  {
139  variable_names_.push_back(name_order[j]);
140  variable_names_set_.insert(name_order[j]);
141  }
142 
143  int first_index = joint_model_vector_[i]->getFirstVariableIndex();
144  for (std::size_t j = 0; j < name_order.size(); ++j)
145  {
146  variable_index_list_.push_back(first_index + j);
147  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
148  }
149  joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
150 
151  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
152  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
153  continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
154 
155  variable_count_ += vc;
156  }
157  else
158  fixed_joints_.push_back(joint_model_vector_[i]);
159  }
160 
161  // now we need to find all the set of joints within this group
162  // that root distinct subtrees
163  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
164  {
165  // if we find that an ancestor is also in the group, then the joint is not a root
166  if (!includesParent(active_joint_model_vector_[i], this))
168  }
169 
170  // when updating this group within a state, it is useful to know
171  // if the full state of a group is contiguous within the full state of the robot
172  if (variable_index_list_.empty())
174  else
175  for (std::size_t i = 1; i < variable_index_list_.size(); ++i)
176  if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
177  {
179  break;
180  }
181 
182  // when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
183  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
184  // if the joint we mimic is also in this group, we will need to do updates when sampling
185  if (hasJointModel(mimic_joints_[i]->getMimic()->getName()))
186  {
187  int src = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
188  int dest = joint_variables_index_map_[mimic_joints_[i]->getName()];
189  GroupMimicUpdate mu(src, dest, mimic_joints_[i]->getMimicFactor(), mimic_joints_[i]->getMimicOffset());
190  group_mimic_update_.push_back(mu);
191  }
192 
193  // now we need to make another pass for group links (we include the fixed joints here)
194  std::set<const LinkModel*> group_links_set;
195  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
196  group_links_set.insert(joint_model_vector_[i]->getChildLinkModel());
197  for (std::set<const LinkModel*>::iterator it = group_links_set.begin(); it != group_links_set.end(); ++it)
198  link_model_vector_.push_back(*it);
199  std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
200 
201  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
202  {
203  link_model_map_[link_model_vector_[i]->getName()] = link_model_vector_[i];
204  link_model_name_vector_.push_back(link_model_vector_[i]->getName());
205  if (!link_model_vector_[i]->getShapes().empty())
206  {
207  link_model_with_geometry_vector_.push_back(link_model_vector_[i]);
208  link_model_with_geometry_name_vector_.push_back(link_model_vector_[i]->getName());
209  }
210  }
211 
212  // compute the common root of this group
213  if (!joint_roots_.empty())
214  {
216  for (std::size_t i = 1; i < joint_roots_.size(); ++i)
218  }
219 
220  // compute updated links
221  for (std::size_t i = 0; i < joint_roots_.size(); ++i)
222  {
223  const std::vector<const LinkModel*>& links = joint_roots_[i]->getDescendantLinkModels();
224  updated_link_model_set_.insert(links.begin(), links.end());
225  }
226  for (std::set<const LinkModel*>::iterator it = updated_link_model_set_.begin(); it != updated_link_model_set_.end();
227  ++it)
228  {
229  updated_link_model_name_set_.insert((*it)->getName());
230  updated_link_model_vector_.push_back(*it);
231  if (!(*it)->getShapes().empty())
232  {
235  updated_link_model_with_geometry_name_set_.insert((*it)->getName());
236  }
237  }
238  std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
240  OrderLinksByIndex());
241  for (std::size_t i = 0; i < updated_link_model_vector_.size(); ++i)
243  for (std::size_t i = 0; i < updated_link_model_with_geometry_vector_.size(); ++i)
245 
246  // check if this group should actually be a chain
247  if (joint_roots_.size() == 1 && active_joint_model_vector_.size() >= 1)
248  {
249  bool chain = true;
250  // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
251  // we should always get to the parent.
252  for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
253  if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
254  {
255  chain = false;
256  break;
257  }
258  if (chain)
259  is_chain_ = true;
260  }
261 }
262 
264 
265 void JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
266 {
267  subgroup_names_ = subgroups;
268  subgroup_names_set_.clear();
269  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
271 }
272 
273 void JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
274 {
275  sub_groups.resize(subgroup_names_.size());
276  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
277  sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
278 }
279 
280 bool JointModelGroup::hasJointModel(const std::string& joint) const
281 {
282  return joint_model_map_.find(joint) != joint_model_map_.end();
283 }
284 
285 bool JointModelGroup::hasLinkModel(const std::string& link) const
286 {
287  return link_model_map_.find(link) != link_model_map_.end();
288 }
289 
290 const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const
291 {
292  LinkModelMapConst::const_iterator it = link_model_map_.find(name);
293  if (it == link_model_map_.end())
294  {
295  ROS_ERROR_NAMED("robot_model.jmg", "Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
296  return nullptr;
297  }
298  return it->second;
299 }
300 
301 const JointModel* JointModelGroup::getJointModel(const std::string& name) const
302 {
303  JointModelMapConst::const_iterator it = joint_model_map_.find(name);
304  if (it == joint_model_map_.end())
305  {
306  ROS_ERROR_NAMED("robot_model.jmg", "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
307  return nullptr;
308  }
309  return it->second;
310 }
311 
313  const JointBoundsVector& active_joint_bounds) const
314 {
315  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
316  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
318  *active_joint_bounds[i]);
319 
320  updateMimicJoints(values);
321 }
322 
324  const JointBoundsVector& active_joint_bounds, const double* near,
325  double distance) const
326 {
327  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
328  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
330  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
331  near + active_joint_model_start_index_[i], distance);
332  updateMimicJoints(values);
333 }
334 
336  random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds,
337  const double* near, const std::map<JointModel::JointType, double>& distance_map) const
338 {
339  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
340  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
341  {
342  double distance = 0.0;
343  std::map<JointModel::JointType, double>::const_iterator iter =
344  distance_map.find(active_joint_model_vector_[i]->getType());
345  if (iter != distance_map.end())
346  distance = iter->second;
347  else
348  ROS_WARN_NAMED("robot_model.jmg", "Did not pass in distance for '%s'",
349  active_joint_model_vector_[i]->getName().c_str());
350  active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(
351  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
352  near + active_joint_model_start_index_[i], distance);
353  }
354  updateMimicJoints(values);
355 }
356 
358  const JointBoundsVector& active_joint_bounds, const double* near,
359  const std::vector<double>& distances) const
360 {
361  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
362  if (distances.size() != active_joint_model_vector_.size())
363  throw Exception("When sampling random values nearby for group '" + name_ +
364  "', distances vector should be of size " +
365  boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
366  boost::lexical_cast<std::string>(distances.size()));
367  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
369  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
370  near + active_joint_model_start_index_[i], distances[i]);
371  updateMimicJoints(values);
372 }
373 
374 bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
375  double margin) const
376 {
377  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
378  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
380  *active_joint_bounds[i], margin))
381  return false;
382  return true;
383 }
384 
385 bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
386 {
387  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
388  bool change = false;
389  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
391  *active_joint_bounds[i]))
392  change = true;
393  if (change)
394  updateMimicJoints(state);
395  return change;
396 }
397 
398 double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
399 {
400  double max_distance = 0.0;
401  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
402  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
403  active_joint_model_vector_[j]->getDistanceFactor();
404  return max_distance;
405 }
406 
407 double JointModelGroup::distance(const double* state1, const double* state2) const
408 {
409  double d = 0.0;
410  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
411  d += active_joint_model_vector_[i]->getDistanceFactor() *
413  state2 + active_joint_model_start_index_[i]);
414  return d;
415 }
416 
417 void JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
418 {
419  // we interpolate values only for active joint models (non-mimic)
420  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
424 
425  // now we update mimic as needed
426  updateMimicJoints(state);
427 }
428 
429 void JointModelGroup::updateMimicJoints(double* values) const
430 {
431  // update mimic (only local joints as we are dealing with a local group state)
432  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
433  values[group_mimic_update_[i].dest] =
434  values[group_mimic_update_[i].src] * group_mimic_update_[i].factor + group_mimic_update_[i].offset;
435 }
436 
437 void JointModelGroup::addDefaultState(const std::string& name, const std::map<std::string, double>& default_state)
438 {
439  default_states_[name] = default_state;
440  default_states_names_.push_back(name);
441 }
442 
443 bool JointModelGroup::getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const
444 {
445  std::map<std::string, std::map<std::string, double> >::const_iterator it = default_states_.find(name);
446  if (it == default_states_.end())
447  return false;
448  values = it->second;
449  return true;
450 }
451 
453 {
454  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
456  updateMimicJoints(values);
457 }
458 
459 void JointModelGroup::getVariableDefaultPositions(std::map<std::string, double>& values) const
460 {
461  std::vector<double> tmp(variable_count_);
463  for (std::size_t i = 0; i < variable_names_.size(); ++i)
464  values[variable_names_[i]] = tmp[i];
465 }
466 
467 void JointModelGroup::setEndEffectorName(const std::string& name)
468 {
469  end_effector_name_ = name;
470 }
471 
472 void JointModelGroup::setEndEffectorParent(const std::string& group, const std::string& link)
473 {
474  end_effector_parent_.first = group;
475  end_effector_parent_.second = link;
476 }
477 
478 void JointModelGroup::attachEndEffector(const std::string& eef_name)
479 {
480  attached_end_effector_names_.push_back(eef_name);
481 }
482 
483 bool JointModelGroup::getEndEffectorTips(std::vector<std::string>& tips) const
484 {
485  // Get a vector of tip links
486  std::vector<const LinkModel*> tip_links;
487  if (!getEndEffectorTips(tip_links))
488  return false;
489 
490  // Convert to string names
491  tips.clear();
492  for (std::size_t i = 0; i < tip_links.size(); ++i)
493  {
494  tips.push_back(tip_links[i]->getName());
495  }
496  return true;
497 }
498 
499 bool JointModelGroup::getEndEffectorTips(std::vector<const LinkModel*>& tips) const
500 {
501  for (std::size_t i = 0; i < getAttachedEndEffectorNames().size(); ++i)
502  {
504  if (!eef)
505  {
506  ROS_ERROR_NAMED("robot_model.jmg", "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("robot_model.jmg", "Unable to find end effector link for eef");
515  return false;
516  }
517 
518  tips.push_back(eef_link);
519  }
520  return true;
521 }
522 
524 {
525  std::vector<const LinkModel*> tips;
526  getEndEffectorTips(tips);
527  if (tips.size() == 1)
528  return tips.front();
529  else if (tips.size() > 1)
530  ROS_ERROR_NAMED("robot_model.jmg", "More than one end effector tip found for joint model group, "
531  "so cannot return only one");
532  else
533  ROS_ERROR_NAMED("robot_model.jmg", "No end effector tips found in joint model group");
534  return nullptr;
535 }
536 
537 int JointModelGroup::getVariableGroupIndex(const std::string& variable) const
538 {
539  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
540  if (it == joint_variables_index_map_.end())
541  {
542  ROS_ERROR_NAMED("robot_model.jmg", "Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str());
543  return -1;
544  }
545  return it->second;
546 }
547 
549 {
550  group_kinematics_.first.default_ik_timeout_ = ik_timeout;
551  if (group_kinematics_.first.solver_instance_)
552  group_kinematics_.first.solver_instance_->setDefaultTimeout(ik_timeout);
553  for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
554  it->second.default_ik_timeout_ = ik_timeout;
555 }
556 
557 void JointModelGroup::setDefaultIKAttempts(unsigned int ik_attempts)
558 {
559  group_kinematics_.first.default_ik_attempts_ = ik_attempts;
560  for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
561  it->second.default_ik_attempts_ = ik_attempts;
562 }
563 
564 bool JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
565  std::vector<unsigned int>& joint_bijection) const
566 {
567  joint_bijection.clear();
568  for (std::size_t i = 0; i < ik_jnames.size(); ++i)
569  {
570  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jnames[i]);
571  if (it == joint_variables_index_map_.end())
572  {
573  // skip reported fixed joints
574  if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED)
575  continue;
576  ROS_ERROR_NAMED("robot_model.jmg", "IK solver computes joint values for joint '%s' "
577  "but group '%s' does not contain such a joint.",
578  ik_jnames[i].c_str(), getName().c_str());
579  return false;
580  }
581  const JointModel* jm = getJointModel(ik_jnames[i]);
582  for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
583  joint_bijection.push_back(it->second + k);
584  }
585  return true;
586 }
587 
588 void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
589 {
590  if (solvers.first)
591  {
592  group_kinematics_.first.allocator_ = solvers.first;
593  group_kinematics_.first.solver_instance_ = solvers.first(this);
594  if (group_kinematics_.first.solver_instance_)
595  {
596  group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
597  group_kinematics_.first.solver_instance_const_ = group_kinematics_.first.solver_instance_;
598  if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
599  group_kinematics_.first.bijection_))
600  group_kinematics_.first.reset();
601  }
602  }
603  else
604  // we now compute a joint bijection only if we have a solver map
605  for (SolverAllocatorMapFn::const_iterator it = solvers.second.begin(); it != solvers.second.end(); ++it)
606  if (it->first->getSolverInstance())
607  {
608  KinematicsSolver& ks = group_kinematics_.second[it->first];
609  ks.allocator_ = it->second;
610  ks.solver_instance_ = const_cast<JointModelGroup*>(it->first)->getSolverInstance();
612  ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
613  ks.default_ik_attempts_ = group_kinematics_.first.default_ik_attempts_;
614  if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
615  {
616  group_kinematics_.second.clear();
617  break;
618  }
619  }
620 }
621 
622 bool JointModelGroup::canSetStateFromIK(const std::string& tip) const
623 {
624  const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
625  if (!solver || tip.empty())
626  return false;
627 
628  const std::vector<std::string>& tip_frames = solver->getTipFrames();
629 
630  if (tip_frames.empty())
631  {
632  ROS_DEBUG_NAMED("robot_model.jmg", "Group %s has no tip frame(s)", name_.c_str());
633  return false;
634  }
635 
636  // loop through all tip frames supported by the JMG
637  for (std::size_t i = 0; i < tip_frames.size(); ++i)
638  {
639  // remove frame reference, if specified
640  const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
641  const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i];
642  ROS_DEBUG_NAMED("robot_model.jmg", "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
643  tip_frame_local.c_str());
644 
645  // Check if the IK solver's tip is the same as the frame of inquiry
646  if (tip_local != tip_frame_local)
647  {
648  // If not the same, check if this planning group includes the frame of inquiry
649  if (hasLinkModel(tip_frame_local))
650  {
651  const LinkModel* lm = getLinkModel(tip_frame_local);
652  const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
653  // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
654  for (LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
655  {
656  if (it->first->getName() == tip_local)
657  return true;
658  }
659  }
660  }
661  else
662  return true;
663  }
664 
665  // Did not find any valid tip frame links to use
666  return false;
667 }
668 
669 void JointModelGroup::printGroupInfo(std::ostream& out) const
670 {
671  out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
672  out << " * Joints:" << std::endl;
673  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
674  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
675  << std::endl;
676  out << " * Variables:" << std::endl;
677  for (std::size_t i = 0; i < variable_names_.size(); ++i)
678  {
679  int local_idx = joint_variables_index_map_.find(variable_names_[i])->second;
681  out << " '" << variable_names_[i] << "', index "
682  << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_names_[i])) << " in full state, index "
683  << local_idx << " in group state";
684  if (jm->getMimic())
685  out << ", mimic '" << jm->getMimic()->getName() << "'";
686  out << std::endl;
687  out << " " << parent_model_->getVariableBounds(variable_names_[i]) << std::endl;
688  }
689  out << " * Variables Index List:" << std::endl;
690  out << " ";
691  for (std::size_t i = 0; i < variable_index_list_.size(); ++i)
692  out << variable_index_list_[i] << " ";
694  out << "(contiguous)";
695  else
696  out << "(non-contiguous)";
697  out << std::endl;
698  if (group_kinematics_.first)
699  {
700  out << " * Kinematics solver bijection:" << std::endl;
701  out << " ";
702  for (std::size_t i = 0; i < group_kinematics_.first.bijection_.size(); ++i)
703  out << group_kinematics_.first.bijection_[i] << " ";
704  out << std::endl;
705  }
706  if (!group_kinematics_.second.empty())
707  {
708  out << " * Compound kinematics solver:" << std::endl;
709  for (KinematicsSolverMap::const_iterator it = group_kinematics_.second.begin();
710  it != group_kinematics_.second.end(); ++it)
711  {
712  out << " " << it->first->getName() << ":";
713  for (std::size_t i = 0; i < it->second.bijection_.size(); ++i)
714  out << " " << it->second.bijection_[i];
715  out << std::endl;
716  }
717  }
718 
719  if (!group_mimic_update_.empty())
720  {
721  out << " * Local Mimic Updates:" << std::endl;
722  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
723  out << " [" << group_mimic_update_[i].dest << "] = " << group_mimic_update_[i].factor << " * ["
724  << group_mimic_update_[i].src << "] + " << group_mimic_update_[i].offset << std::endl;
725  }
726  out << std::endl;
727 }
728 
729 } // end of namespace core
730 } // end of namespace moveit
d
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints. This only updates mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, there are no values to be read (values is only the group state)
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
const kinematics::KinematicsBaseConstPtr & getSolverInstance() const
std::set< const LinkModel * > updated_link_model_with_geometry_set_
The list of downstream link models in the order they should be updated (may include links that are no...
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...
std::vector< GroupMimicUpdate > group_mimic_update_
std::vector< std::string > subgroup_names_
The set of labelled subgroups that are included in this group.
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
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.
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
#define ROS_WARN_NAMED(name,...)
bool enforcePositionBounds(double *state, const JointBoundsVector &active_joint_bounds) const
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:199
std::set< const LinkModel * > updated_link_model_set_
The list of downstream link models in the order they should be updated (may include links that are no...
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...
std::map< std::string, std::map< std::string, double > > default_states_
The set of default states specified for this group in the SRDF.
unsigned int variable_count_
The number of variables necessary to describe this group of joints.
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.
std::set< std::string > variable_names_set_
The names of the DOF that make up this group (this is just a sequence of joint variable names; not ne...
const std::string & getName() const
Get the name of the joint group.
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
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:407
std::set< std::string > updated_link_model_name_set_
The list of downstream link names in the order they should be updated (may include links that are not...
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.
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...
bool canSetStateFromIK(const std::string &tip) const
bool getEndEffectorTips(std::vector< const LinkModel * > &tips) const
Get a vector of end effector tips included in a particular joint model group as defined by the SRDF e...
int getLocalVariableIndex(const std::string &variable) const
Get the index of the variable within this joint.
Definition: joint_model.cpp:85
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
std::vector< const JointModel * > joint_roots_
The list of active joint models that are roots in this group.
std::string name_
Name of group.
std::vector< std::string > joint_model_name_vector_
Names of joints in the order they appear in the group state.
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...
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
std::vector< int > variable_index_list_
The list of index values this group includes, with respect to a full robot state; this includes mimic...
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::vector< std::string > link_model_with_geometry_name_vector_
The names of the links in this group that also have geometry.
std::vector< const LinkModel * > updated_link_model_vector_
The list of downstream link models in the order they should be updated (may include links that are no...
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
std::vector< std::string > default_states_names_
The names of the default states specified for this group in the SRDF.
void setDefaultIKTimeout(double ik_timeout)
Set the default IK timeout.
std::vector< const LinkModel * > link_model_with_geometry_vector_
void interpolate(const double *from, const double *to, double t, double *state) const
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group. ...
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
#define ROS_DEBUG_NAMED(name,...)
std::vector< const JointModel * > mimic_joints_
Joints that mimic other joints.
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
std::vector< std::string > link_model_name_vector_
The names of the links in this group.
std::vector< std::string > updated_link_model_with_geometry_name_vector_
The list of downstream link names in the order they should be updated (may include links that are not...
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel * > &joint_vector, const RobotModel *parent_model)
const RobotModel * parent_model_
Owner model.
std::vector< const JointModel::Bounds * > JointBoundsVector
double getMaximumExtent(const JointBoundsVector &active_joint_bounds) const
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
VariableIndexMap joint_variables_index_map_
The group includes all the joint variables that make up the joints the group consists of...
std::pair< KinematicsSolver, KinematicsSolverMap > group_kinematics_
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
void attachEndEffector(const std::string &eef_name)
Notify this group that there is an end-effector attached to it.
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
std::vector< const JointModel * > fixed_joints_
The joints that have no DOF (fixed)
kinematics::KinematicsBaseConstPtr solver_instance_const_
std::vector< unsigned int > bijection_
The mapping between the order of the joints in the group and the order of the joints in the kinematic...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
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...
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...
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
bool enforcePositionBounds(double *state) const
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
LinkModelMapConst link_model_map_
A map from link names to their instances.
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< unsigned int > &joint_bijection) const
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this group contains.
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
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...
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...
std::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
const JointModel * getCommonRoot(const JointModel *a, const JointModel *b) const
Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument...
Definition: robot_model.h:425
#define ROS_ERROR_NAMED(name,...)
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group) ...
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
double distance(const double *state1, const double *state2) const
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...
std::vector< std::string > updated_link_model_name_vector_
The list of downstream link names in the order they should be updated (may include links that are not...
Main namespace for MoveIt!
std::vector< const LinkModel * > updated_link_model_with_geometry_vector_
The list of downstream link models in the order they should be updated (may include links that are no...
void setDefaultIKAttempts(unsigned int ik_attempts)
Set the default IK attempts.
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137
std::set< std::string > updated_link_model_with_geometry_name_set_
The list of downstream link names in the order they should be updated (may include links that are not...
std::vector< const LinkModel * > link_model_vector_
The links that are on the direct lineage between joints and joint_roots_, as well as the children of ...
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:362


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33