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());
117 
118  // figure out active joints, mimic joints, fixed joints
119  // construct index maps, list of variables
120  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
121  {
123  joint_model_map_[joint_model_vector_[i]->getName()] = joint_model_vector_[i];
124  unsigned int vc = joint_model_vector_[i]->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_vector_[i]->getVariableNames();
130  if (joint_model_vector_[i]->getMimic() == nullptr)
131  {
132  active_joint_model_vector_.push_back(joint_model_vector_[i]);
133  active_joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
135  active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
136  }
137  else
138  mimic_joints_.push_back(joint_model_vector_[i]);
139  for (std::size_t j = 0; j < name_order.size(); ++j)
140  {
141  variable_names_.push_back(name_order[j]);
142  variable_names_set_.insert(name_order[j]);
143  }
144 
145  int first_index = joint_model_vector_[i]->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_vector_[i]->getName()] = variable_count_;
152 
153  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
154  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
155  continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
156 
157  variable_count_ += vc;
158  }
159  else
160  fixed_joints_.push_back(joint_model_vector_[i]);
161  }
162 
163  // now we need to find all the set of joints within this group
164  // that root distinct subtrees
165  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
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_vector_[i], this))
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())
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  {
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 (std::size_t i = 0; i < mimic_joints_.size(); ++i)
186  // if the joint we mimic is also in this group, we will need to do updates when sampling
187  if (hasJointModel(mimic_joints_[i]->getMimic()->getName()))
188  {
189  int src = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
190  int dest = joint_variables_index_map_[mimic_joints_[i]->getName()];
191  GroupMimicUpdate mu(src, dest, mimic_joints_[i]->getMimicFactor(), mimic_joints_[i]->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 (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
198  group_links_set.insert(joint_model_vector_[i]->getChildLinkModel());
199  for (std::set<const LinkModel*>::iterator it = group_links_set.begin(); it != group_links_set.end(); ++it)
200  link_model_vector_.push_back(*it);
201  std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
202 
203  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
204  {
205  link_model_map_[link_model_vector_[i]->getName()] = link_model_vector_[i];
206  link_model_name_vector_.push_back(link_model_vector_[i]->getName());
207  if (!link_model_vector_[i]->getShapes().empty())
208  {
209  link_model_with_geometry_vector_.push_back(link_model_vector_[i]);
210  link_model_with_geometry_name_vector_.push_back(link_model_vector_[i]->getName());
211  }
212  }
213 
214  // compute the common root of this group
215  if (!joint_roots_.empty())
216  {
218  for (std::size_t i = 1; i < joint_roots_.size(); ++i)
220  }
221 
222  // compute updated links
223  for (std::size_t i = 0; i < joint_roots_.size(); ++i)
224  {
225  const std::vector<const LinkModel*>& links = joint_roots_[i]->getDescendantLinkModels();
226  updated_link_model_set_.insert(links.begin(), links.end());
227  }
228  for (std::set<const LinkModel*>::iterator it = updated_link_model_set_.begin(); it != updated_link_model_set_.end();
229  ++it)
230  {
231  updated_link_model_name_set_.insert((*it)->getName());
232  updated_link_model_vector_.push_back(*it);
233  if (!(*it)->getShapes().empty())
234  {
237  updated_link_model_with_geometry_name_set_.insert((*it)->getName());
238  }
239  }
240  std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
242  OrderLinksByIndex());
243  for (std::size_t i = 0; i < updated_link_model_vector_.size(); ++i)
245  for (std::size_t i = 0; i < updated_link_model_with_geometry_vector_.size(); ++i)
247 
248  // check if this group should actually be a chain
249  if (joint_roots_.size() == 1 && !active_joint_model_vector_.empty())
250  {
251  bool chain = true;
252  // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
253  // we should always get to the parent.
254  for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
255  if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
256  {
257  chain = false;
258  break;
259  }
260  if (chain)
261  is_chain_ = true;
262  }
263 }
264 
266 
267 void JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
268 {
269  subgroup_names_ = subgroups;
270  subgroup_names_set_.clear();
271  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
273 }
274 
275 void JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
276 {
277  sub_groups.resize(subgroup_names_.size());
278  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
279  sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
280 }
281 
282 bool JointModelGroup::hasJointModel(const std::string& joint) const
283 {
284  return joint_model_map_.find(joint) != joint_model_map_.end();
285 }
286 
287 bool JointModelGroup::hasLinkModel(const std::string& link) const
288 {
289  return link_model_map_.find(link) != link_model_map_.end();
290 }
291 
292 const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const
293 {
294  LinkModelMapConst::const_iterator it = link_model_map_.find(name);
295  if (it == link_model_map_.end())
296  {
297  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
298  return nullptr;
299  }
300  return it->second;
301 }
302 
303 const JointModel* JointModelGroup::getJointModel(const std::string& name) const
304 {
305  JointModelMapConst::const_iterator it = joint_model_map_.find(name);
306  if (it == joint_model_map_.end())
307  {
308  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
309  return nullptr;
310  }
311  return it->second;
312 }
313 
315  const JointBoundsVector& active_joint_bounds) const
316 {
317  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
318  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
320  *active_joint_bounds[i]);
321 
322  updateMimicJoints(values);
323 }
324 
326  const JointBoundsVector& active_joint_bounds, const double* near,
327  double distance) const
328 {
329  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
330  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
332  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
333  near + active_joint_model_start_index_[i], distance);
334  updateMimicJoints(values);
335 }
336 
338  random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds,
339  const double* near, 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(
352  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
353  near + active_joint_model_start_index_[i], distance);
354  }
355  updateMimicJoints(values);
356 }
357 
359  const JointBoundsVector& active_joint_bounds, const double* near,
360  const std::vector<double>& distances) const
361 {
362  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
363  if (distances.size() != active_joint_model_vector_.size())
364  throw Exception("When sampling random values nearby for group '" + name_ +
365  "', 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  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
371  near + active_joint_model_start_index_[i], distances[i]);
372  updateMimicJoints(values);
373 }
374 
375 bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
376  double margin) const
377 {
378  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
379  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
381  *active_joint_bounds[i], margin))
382  return false;
383  return true;
384 }
385 
386 bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
387 {
388  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
389  bool change = false;
390  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
392  *active_joint_bounds[i]))
393  change = true;
394  if (change)
395  updateMimicJoints(state);
396  return change;
397 }
398 
399 double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
400 {
401  double max_distance = 0.0;
402  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
403  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
404  active_joint_model_vector_[j]->getDistanceFactor();
405  return max_distance;
406 }
407 
408 double JointModelGroup::distance(const double* state1, const double* state2) const
409 {
410  double d = 0.0;
411  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
412  d += active_joint_model_vector_[i]->getDistanceFactor() *
414  state2 + active_joint_model_start_index_[i]);
415  return d;
416 }
417 
418 void JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
419 {
420  // we interpolate values only for active joint models (non-mimic)
421  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
425 
426  // now we update mimic as needed
427  updateMimicJoints(state);
428 }
429 
430 void JointModelGroup::updateMimicJoints(double* values) const
431 {
432  // update mimic (only local joints as we are dealing with a local group state)
433  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
434  values[group_mimic_update_[i].dest] =
435  values[group_mimic_update_[i].src] * group_mimic_update_[i].factor + group_mimic_update_[i].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)
457  updateMimicJoints(values);
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 {
470  end_effector_name_ = name;
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  {
503  const JointModelGroup* eef = parent_model_->getEndEffector(name);
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 
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 (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
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 (std::size_t i = 0; i < ik_jnames.size(); ++i)
564  {
565  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jnames[i]);
566  if (it == joint_variables_index_map_.end())
567  {
568  // skip reported fixed joints
569  if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED)
570  continue;
571  ROS_ERROR_NAMED(LOGNAME, "IK solver computes joint values for joint '%s' "
572  "but group '%s' does not contain such a joint.",
573  ik_jnames[i].c_str(), getName().c_str());
574  return false;
575  }
576  const JointModel* jm = getJointModel(ik_jnames[i]);
577  for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
578  joint_bijection.push_back(it->second + k);
579  }
580  return true;
581 }
582 
583 void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
584 {
585  if (solvers.first)
586  {
587  group_kinematics_.first.allocator_ = solvers.first;
588  group_kinematics_.first.solver_instance_ = solvers.first(this);
589  if (group_kinematics_.first.solver_instance_)
590  {
591  group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
592  if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
593  group_kinematics_.first.bijection_))
594  group_kinematics_.first.reset();
595  }
596  }
597  else
598  // we now compute a joint bijection only if we have a solver map
599  for (SolverAllocatorMapFn::const_iterator it = solvers.second.begin(); it != solvers.second.end(); ++it)
600  if (it->first->getSolverInstance())
601  {
602  KinematicsSolver& ks = group_kinematics_.second[it->first];
603  ks.allocator_ = it->second;
604  ks.solver_instance_ = const_cast<JointModelGroup*>(it->first)->getSolverInstance();
605  ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
606  if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
607  {
608  group_kinematics_.second.clear();
609  break;
610  }
611  }
612 }
613 
614 bool JointModelGroup::canSetStateFromIK(const std::string& tip) const
615 {
616  const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
617  if (!solver || tip.empty())
618  return false;
619 
620  const std::vector<std::string>& tip_frames = solver->getTipFrames();
621 
622  if (tip_frames.empty())
623  {
624  ROS_DEBUG_NAMED(LOGNAME, "Group %s has no tip frame(s)", name_.c_str());
625  return false;
626  }
627 
628  // loop through all tip frames supported by the JMG
629  for (std::size_t i = 0; i < tip_frames.size(); ++i)
630  {
631  // remove frame reference, if specified
632  const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
633  const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i];
634  ROS_DEBUG_NAMED(LOGNAME, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
635  tip_frame_local.c_str());
636 
637  // Check if the IK solver's tip is the same as the frame of inquiry
638  if (tip_local != tip_frame_local)
639  {
640  // If not the same, check if this planning group includes the frame of inquiry
641  if (hasLinkModel(tip_frame_local))
642  {
643  const LinkModel* lm = getLinkModel(tip_frame_local);
644  const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
645  // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
646  for (LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
647  {
648  if (it->first->getName() == tip_local)
649  return true;
650  }
651  }
652  }
653  else
654  return true;
655  }
656 
657  // Did not find any valid tip frame links to use
658  return false;
659 }
660 
661 void JointModelGroup::printGroupInfo(std::ostream& out) const
662 {
663  out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
664  out << " * Joints:" << std::endl;
665  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
666  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
667  << std::endl;
668  out << " * Variables:" << std::endl;
669  for (std::size_t i = 0; i < variable_names_.size(); ++i)
670  {
671  int local_idx = joint_variables_index_map_.find(variable_names_[i])->second;
673  out << " '" << variable_names_[i] << "', index "
674  << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_names_[i])) << " 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_names_[i]) << std::endl;
680  }
681  out << " * Variables Index List:" << std::endl;
682  out << " ";
683  for (std::size_t i = 0; i < variable_index_list_.size(); ++i)
684  out << variable_index_list_[i] << " ";
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 (std::size_t i = 0; i < group_kinematics_.first.bijection_.size(); ++i)
695  out << group_kinematics_.first.bijection_[i] << " ";
696  out << std::endl;
697  }
698  if (!group_kinematics_.second.empty())
699  {
700  out << " * Compound kinematics solver:" << std::endl;
701  for (KinematicsSolverMap::const_iterator it = group_kinematics_.second.begin();
702  it != group_kinematics_.second.end(); ++it)
703  {
704  out << " " << it->first->getName() << ":";
705  for (std::size_t i = 0; i < it->second.bijection_.size(); ++i)
706  out << " " << it->second.bijection_[i];
707  out << std::endl;
708  }
709  }
710 
711  if (!group_mimic_update_.empty())
712  {
713  out << " * Local Mimic Updates:" << std::endl;
714  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
715  out << " [" << group_mimic_update_[i].dest << "] = " << group_mimic_update_[i].factor << " * ["
716  << group_mimic_update_[i].src << "] + " << group_mimic_update_[i].offset << std::endl;
717  }
718  out << std::endl;
719 }
720 
721 bool JointModelGroup::isValidVelocityMove(const std::vector<double>& from_joint_pose,
722  const std::vector<double>& to_joint_pose, double dt) const
723 {
724  // Check for equal sized arrays
725  if (from_joint_pose.size() != to_joint_pose.size())
726  {
727  ROS_ERROR_NAMED(LOGNAME, "To and from joint poses are of different sizes.");
728  return false;
729  }
730 
731  return isValidVelocityMove(&from_joint_pose[0], &to_joint_pose[0], from_joint_pose.size(), dt);
732 }
733 
734 bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose,
735  std::size_t array_size, double dt) const
736 {
737  const std::vector<const JointModel::Bounds*>& bounds = getActiveJointModelsBounds();
738  const std::vector<unsigned int>& bij = getKinematicsSolverJointBijection();
739 
740  for (std::size_t i = 0; i < array_size; ++i)
741  {
742  double dtheta = std::abs(from_joint_pose[i] - to_joint_pose[i]);
743  const std::vector<moveit::core::VariableBounds>* var_bounds = bounds[bij[i]];
744 
745  if (var_bounds->size() != 1)
746  {
747  // TODO(davetcoleman) Support multiple variables
748  ROS_ERROR_NAMED(LOGNAME, "Attempting to check velocity bounds for waypoint move with joints that have multiple "
749  "variables");
750  return false;
751  }
752  const double max_velocity = (*var_bounds)[0].max_velocity_;
753 
754  double max_dtheta = dt * max_velocity;
755  if (dtheta > max_dtheta)
756  {
757  ROS_DEBUG_STREAM_NAMED(LOGNAME, "Not valid velocity move because of joint " << i);
758  return false;
759  }
760  }
761 
762  return true;
763 }
764 } // end of namespace core
765 } // end of namespace moveit
d
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 ...
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.
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...
double getMaximumExtent(const JointBoundsVector &active_joint_bounds) const
std::vector< GroupMimicUpdate > group_mimic_update_
std::vector< std::string > subgroup_names_
The set of labelled subgroups that are included in this group.
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
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...
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:199
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
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...
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
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.
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...
void getSubgroups(std::vector< const JointModelGroup *> &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
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...
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
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...
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 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.
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.
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_
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,...)
int getLocalVariableIndex(const std::string &variable) const
Get the index of the variable within this joint.
Definition: joint_model.cpp:85
bool enforcePositionBounds(double *state) const
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
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.
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
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...
const RobotModel * parent_model_
Owner model.
std::vector< const JointModel::Bounds * > JointBoundsVector
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
const std::string & getName() const
Get the name of the joint group.
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_
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< unsigned int > &joint_bijection) const
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.
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...
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)
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
std::vector< const JointModel * > fixed_joints_
The joints that have no DOF (fixed)
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 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...
const std::string LOGNAME
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.
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
LinkModelMapConst link_model_map_
A map from link names to their instances.
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.
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.
#define ROS_ERROR_NAMED(name,...)
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
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.
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group) ...
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel *> &joint_vector, const RobotModel *parent_model)
double distance(const double *state1, const double *state2) const
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
bool enforcePositionBounds(double *state, const JointBoundsVector &active_joint_bounds) 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 interpolate(const double *from, const double *to, double t, double *state) const
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...
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::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 ...
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:370
bool canSetStateFromIK(const std::string &tip) const
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 11 2020 03:51:21