robot_model.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 */
37 
40 #include <boost/math/constants/constants.hpp>
42 #include <algorithm>
43 #include <limits>
44 #include <queue>
45 #include <cmath>
46 #include <memory>
47 #include "order_robot_model_items.inc"
48 
49 namespace moveit
50 {
51 namespace core
52 {
53 RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model)
54 {
55  root_joint_ = nullptr;
56  urdf_ = urdf_model;
57  srdf_ = srdf_model;
58  buildModel(*urdf_model, *srdf_model);
59 }
60 
62 {
63  for (JointModelGroupMap::iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
64  delete it->second;
65  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
66  delete joint_model_vector_[i];
67  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
68  delete link_model_vector_[i];
69 }
70 
72 {
73  return root_joint_;
74 }
75 
77 {
78  return root_link_;
79 }
80 
81 void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model)
82 {
84  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildModel");
85 
86  root_joint_ = nullptr;
87  root_link_ = nullptr;
89  variable_count_ = 0;
90  model_name_ = urdf_model.getName();
91  ROS_INFO_NAMED("robot_model", "Loading robot model '%s'...", model_name_.c_str());
92 
93  if (urdf_model.getRoot())
94  {
95  const urdf::Link* root_link_ptr = urdf_model.getRoot().get();
96  model_frame_ = '/' + root_link_ptr->name;
97 
98  ROS_DEBUG_NAMED("robot_model", "... building kinematic chain");
99  root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model);
100  if (root_joint_)
102  ROS_DEBUG_NAMED("robot_model", "... building mimic joints");
103  buildMimic(urdf_model);
104 
105  ROS_DEBUG_NAMED("robot_model", "... computing joint indexing");
106  buildJointInfo();
107 
109  ROS_WARN_NAMED("robot_model", "No geometry is associated to any robot links");
110 
111  // build groups
112 
113  ROS_DEBUG_NAMED("robot_model", "... constructing joint groups");
114  buildGroups(srdf_model);
115 
116  ROS_DEBUG_NAMED("robot_model", "... constructing joint group states");
117  buildGroupStates(srdf_model);
118 
119  // For debugging entire model
120  // printModelInfo(std::cout);
121  }
122  else
123  ROS_WARN_NAMED("robot_model", "No root link found");
124 }
125 
126 namespace
127 {
128 typedef std::map<const JointModel*, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
129  std::set<const JointModel*, OrderJointsByIndex> > >
130  DescMap;
131 
132 void computeDescendantsHelper(const JointModel* joint, std::vector<const JointModel*>& parents,
133  std::set<const JointModel*>& seen, DescMap& descendants)
134 {
135  if (!joint)
136  return;
137  if (seen.find(joint) != seen.end())
138  return;
139  seen.insert(joint);
140 
141  for (std::size_t i = 0; i < parents.size(); ++i)
142  descendants[parents[i]].second.insert(joint);
143 
144  const LinkModel* lm = joint->getChildLinkModel();
145  if (!lm)
146  return;
147 
148  for (std::size_t i = 0; i < parents.size(); ++i)
149  descendants[parents[i]].first.insert(lm);
150  descendants[joint].first.insert(lm);
151 
152  parents.push_back(joint);
153  const std::vector<const JointModel*>& ch = lm->getChildJointModels();
154  for (std::size_t i = 0; i < ch.size(); ++i)
155  computeDescendantsHelper(ch[i], parents, seen, descendants);
156  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
157  for (std::size_t i = 0; i < mim.size(); ++i)
158  computeDescendantsHelper(mim[i], parents, seen, descendants);
159  parents.pop_back();
160 }
161 
162 void computeCommonRootsHelper(const JointModel* joint, std::vector<int>& common_roots, int size)
163 {
164  if (!joint)
165  return;
166  const LinkModel* lm = joint->getChildLinkModel();
167  if (!lm)
168  return;
169 
170  const std::vector<const JointModel*>& ch = lm->getChildJointModels();
171  for (std::size_t i = 0; i < ch.size(); ++i)
172  {
173  const std::vector<const JointModel*>& a = ch[i]->getDescendantJointModels();
174  for (std::size_t j = i + 1; j < ch.size(); ++j)
175  {
176  const std::vector<const JointModel*>& b = ch[j]->getDescendantJointModels();
177  for (std::size_t m = 0; m < b.size(); ++m)
178  common_roots[ch[i]->getJointIndex() * size + b[m]->getJointIndex()] =
179  common_roots[ch[i]->getJointIndex() + b[m]->getJointIndex() * size] = joint->getJointIndex();
180  for (std::size_t k = 0; k < a.size(); ++k)
181  {
182  common_roots[a[k]->getJointIndex() * size + ch[j]->getJointIndex()] =
183  common_roots[a[k]->getJointIndex() + ch[j]->getJointIndex() * size] = joint->getJointIndex();
184  for (std::size_t m = 0; m < b.size(); ++m)
185  common_roots[a[k]->getJointIndex() * size + b[m]->getJointIndex()] =
186  common_roots[a[k]->getJointIndex() + b[m]->getJointIndex() * size] = joint->getJointIndex();
187  }
188  }
189  computeCommonRootsHelper(ch[i], common_roots, size);
190  }
191 }
192 }
193 
195 {
196  // compute common roots for all pairs of joints;
197  // there are 3 cases of pairs (X, Y):
198  // X != Y && X and Y are not descendants of one another
199  // X == Y
200  // X != Y && X and Y are descendants of one another
201 
202  // by default, the common root is always the global root;
204 
205  // look at all descendants recursively; for two sibling nodes A, B, both children of X, all the pairs of respective
206  // descendants of A and B
207  // have X as the common root.
208  computeCommonRootsHelper(root_joint_, common_joint_roots_, joint_model_vector_.size());
209 
210  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
211  {
212  // the common root of a joint and itself is the same joint:
213  common_joint_roots_[joint_model_vector_[i]->getJointIndex() * (1 + joint_model_vector_.size())] =
214  joint_model_vector_[i]->getJointIndex();
215 
216  // a node N and one of its descendants have as common root the node N itself:
217  const std::vector<const JointModel*>& d = joint_model_vector_[i]->getDescendantJointModels();
218  for (std::size_t j = 0; j < d.size(); ++j)
219  common_joint_roots_[d[j]->getJointIndex() * joint_model_vector_.size() +
220  joint_model_vector_[i]->getJointIndex()] =
221  common_joint_roots_[d[j]->getJointIndex() +
222  joint_model_vector_[i]->getJointIndex() * joint_model_vector_.size()] =
223  joint_model_vector_[i]->getJointIndex();
224  }
225 }
226 
228 {
229  // compute the list of descendants for all joints
230  std::vector<const JointModel*> parents;
231  std::set<const JointModel*> seen;
232 
233  DescMap descendants;
234  computeDescendantsHelper(root_joint_, parents, seen, descendants);
235  for (DescMap::iterator it = descendants.begin(); it != descendants.end(); ++it)
236  {
237  JointModel* jm = const_cast<JointModel*>(it->first);
238  for (std::set<const JointModel*>::const_iterator jt = it->second.second.begin(); jt != it->second.second.end();
239  ++jt)
240  jm->addDescendantJointModel(*jt);
241  for (std::set<const LinkModel*>::const_iterator jt = it->second.first.begin(); jt != it->second.first.end(); ++jt)
242  jm->addDescendantLinkModel(*jt);
243  }
244 }
245 
247 {
249  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildJointInfo");
250 
251  // construct additional maps for easy access by name
252  variable_count_ = 0;
254  variable_names_.reserve(joint_model_vector_.size());
256 
257  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
258  {
259  joint_model_vector_[i]->setJointIndex(i);
260  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
261 
262  // compute index map
263  if (!name_order.empty())
264  {
265  for (std::size_t j = 0; j < name_order.size(); ++j)
266  {
267  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
268  variable_names_.push_back(name_order[j]);
270  }
271  if (joint_model_vector_[i]->getMimic() == nullptr)
272  {
277  }
278 
279  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
280  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
282 
283  joint_model_vector_[i]->setFirstVariableIndex(variable_count_);
285 
286  // compute variable count
287  std::size_t vc = joint_model_vector_[i]->getVariableCount();
288  variable_count_ += vc;
289  if (vc == 1)
290  single_dof_joints_.push_back(joint_model_vector_[i]);
291  else
292  multi_dof_joints_.push_back(joint_model_vector_[i]);
293  }
294  }
295 
296  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
297  {
298  LinkTransformMap associated_transforms;
299  computeFixedTransforms(link_model_vector_[i], link_model_vector_[i]->getJointOriginTransform().inverse(),
300  associated_transforms);
301  if (associated_transforms.size() > 1)
302  {
303  for (LinkTransformMap::iterator it = associated_transforms.begin(); it != associated_transforms.end(); ++it)
304  if (it->first != link_model_vector_[i])
305  {
306  getLinkModel(it->first->getName())->addAssociatedFixedTransform(link_model_vector_[i], it->second.inverse());
307  link_model_vector_[i]->addAssociatedFixedTransform(it->first, it->second);
308  }
309  }
310  }
311 
313  computeCommonRoots(); // must be called _after_ list of descendants was computed
314 }
315 
317 {
318  // copy the default states to the groups
319  const std::vector<srdf::Model::GroupState>& ds = srdf_model.getGroupStates();
320  for (std::size_t i = 0; i < ds.size(); ++i)
321  {
322  if (hasJointModelGroup(ds[i].group_))
323  {
324  JointModelGroup* jmg = getJointModelGroup(ds[i].group_);
325  std::map<std::string, double> state;
326  for (std::map<std::string, std::vector<double> >::const_iterator jt = ds[i].joint_values_.begin();
327  jt != ds[i].joint_values_.end(); ++jt)
328  {
329  if (jmg->hasJointModel(jt->first))
330  {
331  const JointModel* jm = jmg->getJointModel(jt->first);
332  const std::vector<std::string>& vn = jm->getVariableNames();
333  if (vn.size() == jt->second.size())
334  for (std::size_t j = 0; j < vn.size(); ++j)
335  state[vn[j]] = jt->second[j];
336  else
338  "robot_model", "The model for joint '%s' requires %d variable values, "
339  "but only %d variable values were supplied in default state '%s' for group '%s'",
340  jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(), jmg->getName().c_str());
341  }
342  else
343  ROS_ERROR_NAMED("robot_model", "Group state '%s' specifies value for joint '%s', "
344  "but that joint is not part of group '%s'",
345  ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
346  }
347  if (!state.empty())
348  jmg->addDefaultState(ds[i].name_, state);
349  }
350  else
351  ROS_ERROR_NAMED("robot_model", "Group state '%s' specified for group '%s', but that group does not exist",
352  ds[i].name_.c_str(), ds[i].group_.c_str());
353  }
354 }
355 
356 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
357 {
358  // compute mimic joints
359  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
360  {
361  const urdf::Joint* jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
362  if (jm)
363  if (jm->mimic)
364  {
365  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
366  if (jit != joint_model_map_.end())
367  {
368  if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
369  joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
370  else
371  ROS_ERROR_NAMED("robot_model",
372  "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
373  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
374  }
375  else
376  ROS_ERROR_NAMED("robot_model", "Joint '%s' cannot mimic unknown joint '%s'",
377  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
378  }
379  }
380 
381  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
382  bool change = true;
383  while (change)
384  {
385  change = false;
386  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
387  if (joint_model_vector_[i]->getMimic())
388  {
389  if (joint_model_vector_[i]->getMimic()->getMimic())
390  {
391  joint_model_vector_[i]->setMimic(
392  joint_model_vector_[i]->getMimic()->getMimic(),
393  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicFactor(),
394  joint_model_vector_[i]->getMimicOffset() +
395  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicOffset());
396  change = true;
397  }
398  if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic())
399  {
400  ROS_ERROR_NAMED("robot_model", "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
401  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
402  joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0);
403  change = false;
404  break;
405  }
406  }
407  }
408  // build mimic requests
409  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
410  if (joint_model_vector_[i]->getMimic())
411  {
412  const_cast<JointModel*>(joint_model_vector_[i]->getMimic())->addMimicRequest(joint_model_vector_[i]);
413  mimic_joints_.push_back(joint_model_vector_[i]);
414  }
415 }
416 
417 bool RobotModel::hasEndEffector(const std::string& eef) const
418 {
419  return end_effectors_map_.find(eef) != end_effectors_map_.end();
420 }
421 
422 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
423 {
424  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
425  if (it == end_effectors_map_.end())
426  {
427  it = joint_model_group_map_.find(name);
428  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
429  return it->second;
430  ROS_ERROR_NAMED("robot_model", "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
431  return nullptr;
432  }
433  return it->second;
434 }
435 
437 {
438  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
439  if (it == end_effectors_map_.end())
440  {
441  it = joint_model_group_map_.find(name);
442  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
443  return it->second;
444  ROS_ERROR_NAMED("robot_model", "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
445  return nullptr;
446  }
447  return it->second;
448 }
449 
450 bool RobotModel::hasJointModelGroup(const std::string& name) const
451 {
452  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
453 }
454 
455 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
456 {
457  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
458  if (it == joint_model_group_map_.end())
459  {
460  ROS_ERROR_NAMED("robot_model", "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
461  return nullptr;
462  }
463  return it->second;
464 }
465 
467 {
468  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
469  if (it == joint_model_group_map_.end())
470  {
471  ROS_ERROR_NAMED("robot_model", "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
472  return nullptr;
473  }
474  return it->second;
475 }
476 
477 void RobotModel::buildGroups(const srdf::Model& srdf_model)
478 {
479  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
480 
481  // the only thing tricky is dealing with subgroups
482  std::vector<bool> processed(group_configs.size(), false);
483 
484  bool added = true;
485  while (added)
486  {
487  added = false;
488 
489  // going to make passes until we can't do anything else
490  for (std::size_t i = 0; i < group_configs.size(); ++i)
491  if (!processed[i])
492  {
493  // if we haven't processed, check and see if the dependencies are met yet
494  bool all_subgroups_added = true;
495  for (std::size_t j = 0; j < group_configs[i].subgroups_.size(); ++j)
496  if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
497  {
498  all_subgroups_added = false;
499  break;
500  }
501  if (all_subgroups_added)
502  {
503  added = true;
504  processed[i] = true;
505  if (!addJointModelGroup(group_configs[i]))
506  ROS_WARN_NAMED("robot_model", "Failed to add group '%s'", group_configs[i].name_.c_str());
507  }
508  }
509  }
510 
511  for (std::size_t i = 0; i < processed.size(); ++i)
512  if (!processed[i])
513  ROS_WARN_NAMED("robot_model", "Could not process group '%s' due to unmet subgroup dependencies",
514  group_configs[i].name_.c_str());
515 
516  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
517  joint_model_groups_.push_back(it->second);
518  std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
519  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
520  {
523  }
524 
525  buildGroupsInfo_Subgroups(srdf_model);
526  buildGroupsInfo_EndEffectors(srdf_model);
527 }
528 
530 {
531  // compute subgroups
532  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
533  {
534  JointModelGroup* jmg = it->second;
535  std::vector<std::string> subgroup_names;
536  std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
537  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
538  ++jt)
539  if (jt->first != it->first)
540  {
541  bool ok = true;
542  JointModelGroup* sub_jmg = jt->second;
543  const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
544  for (std::size_t k = 0; k < sub_joints.size(); ++k)
545  if (joints.find(sub_joints[k]) == joints.end())
546  {
547  ok = false;
548  break;
549  }
550  if (ok)
551  subgroup_names.push_back(sub_jmg->getName());
552  }
553  if (!subgroup_names.empty())
554  jmg->setSubgroupNames(subgroup_names);
555  }
556 }
557 
559 {
560  // set the end-effector flags
561  const std::vector<srdf::Model::EndEffector>& eefs = srdf_model.getEndEffectors();
562  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
563  {
564  // check if this group is a known end effector
565  for (std::size_t k = 0; k < eefs.size(); ++k)
566  if (eefs[k].component_group_ == it->first)
567  {
568  // if it is, mark it as such
569  it->second->setEndEffectorName(eefs[k].name_);
570  end_effectors_map_[eefs[k].name_] = it->second;
571  end_effectors_.push_back(it->second);
572 
573  // check to see if there are groups that contain the parent link of this end effector.
574  // record this information if found;
575  std::vector<JointModelGroup*> possible_parent_groups;
576  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
577  ++jt)
578  if (jt->first != it->first)
579  {
580  if (jt->second->hasLinkModel(eefs[k].parent_link_))
581  {
582  jt->second->attachEndEffector(eefs[k].name_);
583  possible_parent_groups.push_back(jt->second);
584  }
585  }
586 
587  JointModelGroup* eef_parent_group = nullptr;
588  // if a parent group is specified in SRDF, try to use it
589  if (!eefs[k].parent_group_.empty())
590  {
591  JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
592  if (jt != joint_model_group_map_.end())
593  {
594  if (jt->second->hasLinkModel(eefs[k].parent_link_))
595  {
596  if (jt->second != it->second)
597  eef_parent_group = jt->second;
598  else
599  ROS_ERROR_NAMED("robot_model", "Group '%s' for end-effector '%s' cannot be its own parent",
600  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
601  }
602  else
603  ROS_ERROR_NAMED("robot_model", "Group '%s' was specified as parent group for end-effector '%s' "
604  "but it does not include the parent link '%s'",
605  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
606  }
607  else
608  ROS_ERROR_NAMED("robot_model",
609  "Group name '%s' not found (specified as parent group for end-effector '%s')",
610  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
611  }
612 
613  // if no parent group was specified, use a default one
614  if (eef_parent_group == nullptr)
615  if (!possible_parent_groups.empty())
616  {
617  // if there are multiple options for the group that contains this end-effector,
618  // we pick the group with fewest joints.
619  std::size_t best = 0;
620  for (std::size_t g = 1; g < possible_parent_groups.size(); ++g)
621  if (possible_parent_groups[g]->getJointModels().size() <
622  possible_parent_groups[best]->getJointModels().size())
623  best = g;
624  eef_parent_group = possible_parent_groups[best];
625  }
626 
627  if (eef_parent_group)
628  {
629  it->second->setEndEffectorParent(eef_parent_group->getName(), eefs[k].parent_link_);
630  }
631  else
632  {
633  ROS_WARN_NAMED("robot_model", "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
634  it->second->setEndEffectorParent("", eefs[k].parent_link_);
635  }
636  break;
637  }
638  }
639  std::sort(end_effectors_.begin(), end_effectors_.end(), OrderGroupsByName());
640 }
641 
643 {
645  {
646  ROS_WARN_NAMED("robot_model", "A group named '%s' already exists. Not adding.", gc.name_.c_str());
647  return false;
648  }
649 
650  std::set<const JointModel*> jset;
651 
652  // add joints from chains
653  for (std::size_t i = 0; i < gc.chains_.size(); ++i)
654  {
655  const LinkModel* base_link = getLinkModel(gc.chains_[i].first);
656  const LinkModel* tip_link = getLinkModel(gc.chains_[i].second);
657  if (base_link && tip_link)
658  {
659  // go from tip, up the chain, until we hit the root or we find the base_link
660  const LinkModel* lm = tip_link;
661  std::vector<const JointModel*> cj;
662  while (lm)
663  {
664  if (lm == base_link)
665  break;
666  cj.push_back(lm->getParentJointModel());
668  }
669  // if we did not find the base_link, we could have a chain like e.g.,
670  // from one end-effector to another end-effector, so the root is in between
671  if (lm != base_link)
672  {
673  // we go up the chain from the base this time, and see where we intersect the other chain
674  lm = base_link;
675  std::size_t index = 0;
676  std::vector<const JointModel*> cj2;
677  while (lm)
678  {
679  for (std::size_t j = 0; j < cj.size(); ++j)
680  if (cj[j] == lm->getParentJointModel())
681  {
682  index = j + 1;
683  break;
684  }
685  if (index > 0)
686  break;
687  cj2.push_back(lm->getParentJointModel());
689  }
690  if (index > 0)
691  {
692  jset.insert(cj.begin(), cj.begin() + index);
693  jset.insert(cj2.begin(), cj2.end());
694  }
695  }
696  else
697  // if we have a simple chain, just add the joints
698  jset.insert(cj.begin(), cj.end());
699  }
700  }
701 
702  // add joints
703  for (std::size_t i = 0; i < gc.joints_.size(); ++i)
704  {
705  const JointModel* j = getJointModel(gc.joints_[i]);
706  if (j)
707  jset.insert(j);
708  }
709 
710  // add joints that are parents of included links
711  for (std::size_t i = 0; i < gc.links_.size(); ++i)
712  {
713  const LinkModel* l = getLinkModel(gc.links_[i]);
714  if (l)
715  jset.insert(l->getParentJointModel());
716  }
717 
718  // add joints from subgroups
719  for (std::size_t i = 0; i < gc.subgroups_.size(); ++i)
720  {
721  const JointModelGroup* sg = getJointModelGroup(gc.subgroups_[i]);
722  if (sg)
723  {
724  // active joints
725  const std::vector<const JointModel*>& js = sg->getJointModels();
726  for (std::size_t j = 0; j < js.size(); ++j)
727  jset.insert(js[j]);
728 
729  // fixed joints
730  const std::vector<const JointModel*>& fs = sg->getFixedJointModels();
731  for (std::size_t j = 0; j < fs.size(); ++j)
732  jset.insert(fs[j]);
733 
734  // mimic joints
735  const std::vector<const JointModel*>& ms = sg->getMimicJointModels();
736  for (std::size_t j = 0; j < ms.size(); ++j)
737  jset.insert(ms[j]);
738  }
739  }
740 
741  if (jset.empty())
742  {
743  ROS_WARN_NAMED("robot_model", "Group '%s' must have at least one valid joint", gc.name_.c_str());
744  return false;
745  }
746 
747  std::vector<const JointModel*> joints;
748  for (std::set<const JointModel*>::iterator it = jset.begin(); it != jset.end(); ++it)
749  joints.push_back(*it);
750 
751  JointModelGroup* jmg = new JointModelGroup(gc.name_, gc, joints, this);
752  joint_model_group_map_[gc.name_] = jmg;
753 
754  return true;
755 }
756 
757 JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, const srdf::Model& srdf_model)
758 {
759  // construct the joint
760  JointModel* joint = urdf_link->parent_joint ?
761  constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
762  constructJointModel(nullptr, urdf_link, srdf_model);
763  if (joint == nullptr)
764  return nullptr;
765 
766  // bookkeeping for the joint
767  joint_model_map_[joint->getName()] = joint;
768  joint->setJointIndex(joint_model_vector_.size());
769  joint_model_vector_.push_back(joint);
770  joint_model_vector_const_.push_back(joint);
771  joint_model_names_vector_.push_back(joint->getName());
772  joint->setParentLinkModel(parent);
773 
774  // construct the link
775  LinkModel* link = constructLinkModel(urdf_link);
776  joint->setChildLinkModel(link);
777  link->setParentLinkModel(parent);
778 
779  // bookkeeping for the link
780  link_model_map_[joint->getChildLinkModel()->getName()] = link;
781  link->setLinkIndex(link_model_vector_.size());
782  link_model_vector_.push_back(link);
783  link_model_vector_const_.push_back(link);
784  link_model_names_vector_.push_back(link->getName());
785  if (!link->getShapes().empty())
786  {
790  link_geometry_count_ += link->getShapes().size();
791  }
792  link->setParentJointModel(joint);
793 
794  // recursively build child links (and joints)
795  for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i)
796  {
797  JointModel* jm = buildRecursive(link, urdf_link->child_links[i].get(), srdf_model);
798  if (jm)
799  link->addChildJointModel(jm);
800  }
801  return joint;
802 }
803 
804 namespace
805 {
806 // construct bounds for 1DOF joint
807 static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
808 {
809  VariableBounds b;
810  if (urdf_joint->safety)
811  {
812  b.position_bounded_ = true;
813  b.min_position_ = urdf_joint->safety->soft_lower_limit;
814  b.max_position_ = urdf_joint->safety->soft_upper_limit;
815  if (urdf_joint->limits)
816  {
817  if (urdf_joint->limits->lower > b.min_position_)
818  b.min_position_ = urdf_joint->limits->lower;
819  if (urdf_joint->limits->upper < b.max_position_)
820  b.max_position_ = urdf_joint->limits->upper;
821  }
822  }
823  else
824  {
825  if (urdf_joint->limits)
826  {
827  b.position_bounded_ = true;
828  b.min_position_ = urdf_joint->limits->lower;
829  b.max_position_ = urdf_joint->limits->upper;
830  }
831  }
832  if (urdf_joint->limits)
833  {
834  b.max_velocity_ = fabs(urdf_joint->limits->velocity);
836  b.velocity_bounded_ = b.max_velocity_ > std::numeric_limits<double>::epsilon();
837  }
838  return b;
839 }
840 }
841 
842 JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link,
843  const srdf::Model& srdf_model)
844 {
845  JointModel* result = nullptr;
846 
847  // must be the root link transform
848  if (urdf_joint)
849  {
850  switch (urdf_joint->type)
851  {
852  case urdf::Joint::REVOLUTE:
853  {
854  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
855  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
856  j->setContinuous(false);
857  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
858  result = j;
859  }
860  break;
861  case urdf::Joint::CONTINUOUS:
862  {
863  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
864  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
865  j->setContinuous(true);
866  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
867  result = j;
868  }
869  break;
870  case urdf::Joint::PRISMATIC:
871  {
872  PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name);
873  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
874  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
875  result = j;
876  }
877  break;
878  case urdf::Joint::FLOATING:
879  result = new FloatingJointModel(urdf_joint->name);
880  break;
881  case urdf::Joint::PLANAR:
882  result = new PlanarJointModel(urdf_joint->name);
883  break;
884  case urdf::Joint::FIXED:
885  result = new FixedJointModel(urdf_joint->name);
886  break;
887  default:
888  ROS_ERROR_NAMED("robot_model", "Unknown joint type: %d", (int)urdf_joint->type);
889  break;
890  }
891  }
892  else
893  {
894  const std::vector<srdf::Model::VirtualJoint>& vjoints = srdf_model.getVirtualJoints();
895  for (std::size_t i = 0; i < vjoints.size(); ++i)
896  {
897  if (vjoints[i].child_link_ != child_link->name)
898  {
899  ROS_WARN_NAMED("robot_model", "Skipping virtual joint '%s' because its child frame '%s' "
900  "does not match the URDF frame '%s'",
901  vjoints[i].name_.c_str(), vjoints[i].child_link_.c_str(), child_link->name.c_str());
902  }
903  else if (vjoints[i].parent_frame_.empty())
904  {
905  ROS_WARN_NAMED("robot_model", "Skipping virtual joint '%s' because its parent frame is empty",
906  vjoints[i].name_.c_str());
907  }
908  else
909  {
910  if (vjoints[i].type_ == "fixed")
911  result = new FixedJointModel(vjoints[i].name_);
912  else if (vjoints[i].type_ == "planar")
913  result = new PlanarJointModel(vjoints[i].name_);
914  else if (vjoints[i].type_ == "floating")
915  result = new FloatingJointModel(vjoints[i].name_);
916  if (result)
917  {
918  // for fixed frames we still use the robot root link
919  if (vjoints[i].type_ != "fixed")
920  {
921  model_frame_ = vjoints[i].parent_frame_;
922  if (model_frame_[0] != '/')
923  model_frame_ = '/' + model_frame_;
924  }
925  break;
926  }
927  }
928  }
929  if (!result)
930  {
931  ROS_INFO_NAMED("robot_model", "No root/virtual joint specified in SRDF. Assuming fixed joint");
932  result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
933  }
934  }
935 
936  if (result)
937  {
938  result->setDistanceFactor(result->getStateSpaceDimension());
939  const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
940  for (std::size_t i = 0; i < pjoints.size(); ++i)
941  {
942  if (result->getName() == pjoints[i].name_)
943  {
944  result->setPassive(true);
945  break;
946  }
947  }
948  }
949 
950  return result;
951 }
952 
953 namespace
954 {
955 static inline Eigen::Affine3d urdfPose2Affine3d(const urdf::Pose& pose)
956 {
957  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
958  Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q.toRotationMatrix());
959  return af;
960 }
961 }
962 
963 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
964 {
965  LinkModel* result = new LinkModel(urdf_link->name);
966 
967  const std::vector<urdf::CollisionSharedPtr>& col_array =
968  urdf_link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, urdf_link->collision) :
969  urdf_link->collision_array;
970 
971  std::vector<shapes::ShapeConstPtr> shapes;
973 
974  for (std::size_t i = 0; i < col_array.size(); ++i)
975  if (col_array[i] && col_array[i]->geometry)
976  {
977  shapes::ShapeConstPtr s = constructShape(col_array[i]->geometry.get());
978  if (s)
979  {
980  shapes.push_back(s);
981  poses.push_back(urdfPose2Affine3d(col_array[i]->origin));
982  }
983  }
984  if (shapes.empty())
985  {
986  const std::vector<urdf::VisualSharedPtr>& vis_array = urdf_link->visual_array.empty() ?
987  std::vector<urdf::VisualSharedPtr>(1, urdf_link->visual) :
988  urdf_link->visual_array;
989  for (std::size_t i = 0; i < vis_array.size(); ++i)
990  if (vis_array[i] && vis_array[i]->geometry)
991  {
992  shapes::ShapeConstPtr s = constructShape(vis_array[i]->geometry.get());
993  if (s)
994  {
995  shapes.push_back(s);
996  poses.push_back(urdfPose2Affine3d(vis_array[i]->origin));
997  }
998  }
999  }
1000 
1001  result->setGeometry(shapes, poses);
1002 
1003  // figure out visual mesh (try visual urdf tag first, collision tag otherwise
1004  if (urdf_link->visual && urdf_link->visual->geometry)
1005  {
1006  if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
1007  {
1008  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
1009  if (!mesh->filename.empty())
1010  result->setVisualMesh(mesh->filename, urdfPose2Affine3d(urdf_link->visual->origin),
1011  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1012  }
1013  }
1014  else if (urdf_link->collision && urdf_link->collision->geometry)
1015  {
1016  if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
1017  {
1018  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
1019  if (!mesh->filename.empty())
1020  result->setVisualMesh(mesh->filename, urdfPose2Affine3d(urdf_link->collision->origin),
1021  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1022  }
1023  }
1024 
1025  if (urdf_link->parent_joint)
1026  result->setJointOriginTransform(urdfPose2Affine3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
1027 
1028  return result;
1029 }
1030 
1032 {
1033  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
1034 
1035  shapes::Shape* result = nullptr;
1036  switch (geom->type)
1037  {
1038  case urdf::Geometry::SPHERE:
1039  result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
1040  break;
1041  case urdf::Geometry::BOX:
1042  {
1043  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
1044  result = new shapes::Box(dim.x, dim.y, dim.z);
1045  }
1046  break;
1047  case urdf::Geometry::CYLINDER:
1048  result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
1049  static_cast<const urdf::Cylinder*>(geom)->length);
1050  break;
1051  case urdf::Geometry::MESH:
1052  {
1053  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
1054  if (!mesh->filename.empty())
1055  {
1056  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
1057  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
1058  result = m;
1059  }
1060  }
1061  break;
1062  default:
1063  ROS_ERROR_NAMED("robot_model", "Unknown geometry type: %d", (int)geom->type);
1064  break;
1065  }
1066 
1067  return shapes::ShapePtr(result);
1068 }
1069 
1070 bool RobotModel::hasJointModel(const std::string& name) const
1071 {
1072  return joint_model_map_.find(name) != joint_model_map_.end();
1073 }
1074 
1075 bool RobotModel::hasLinkModel(const std::string& name) const
1076 {
1077  return link_model_map_.find(name) != link_model_map_.end();
1078 }
1079 
1080 const JointModel* RobotModel::getJointModel(const std::string& name) const
1081 {
1082  JointModelMap::const_iterator it = joint_model_map_.find(name);
1083  if (it != joint_model_map_.end())
1084  return it->second;
1085  ROS_ERROR_NAMED("robot_model", "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1086  return nullptr;
1087 }
1088 
1089 const JointModel* RobotModel::getJointModel(int index) const
1090 {
1091  if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
1092  {
1093  ROS_ERROR_NAMED("robot_model", "Joint index '%i' out of bounds of joints in model '%s'", index,
1094  model_name_.c_str());
1095  return nullptr;
1096  }
1097  assert(joint_model_vector_[index]->getJointIndex() == index);
1098  return joint_model_vector_[index];
1099 }
1100 
1101 JointModel* RobotModel::getJointModel(const std::string& name)
1102 {
1103  JointModelMap::const_iterator it = joint_model_map_.find(name);
1104  if (it != joint_model_map_.end())
1105  return it->second;
1106  ROS_ERROR_NAMED("robot_model", "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1107  return nullptr;
1108 }
1109 
1110 const LinkModel* RobotModel::getLinkModel(const std::string& name) const
1111 {
1112  return const_cast<RobotModel*>(this)->getLinkModel(name);
1113 }
1114 
1115 const LinkModel* RobotModel::getLinkModel(int index) const
1116 {
1117  if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
1118  {
1119  ROS_ERROR_NAMED("robot_model", "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
1120  return nullptr;
1121  }
1122  assert(link_model_vector_[index]->getLinkIndex() == index);
1123  return link_model_vector_[index];
1124 }
1125 
1126 LinkModel* RobotModel::getLinkModel(const std::string& name)
1127 {
1128  LinkModelMap::const_iterator it = link_model_map_.find(name);
1129  if (it != link_model_map_.end())
1130  return it->second;
1131  ROS_ERROR_NAMED("robot_model", "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1132  return nullptr;
1133 }
1134 
1136 {
1137  if (!link)
1138  return link;
1139  const robot_model::LinkModel* parent_link = link->getParentLinkModel();
1140  const robot_model::JointModel* joint = link->getParentJointModel();
1141 
1142  while (parent_link && joint->getType() == robot_model::JointModel::FIXED)
1143  {
1144  link = parent_link;
1145  joint = link->getParentJointModel();
1146  parent_link = joint->getParentLinkModel();
1147  }
1148  return link;
1149 }
1150 
1151 void RobotModel::updateMimicJoints(double* values) const
1152 {
1153  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
1154  {
1155  int src = mimic_joints_[i]->getMimic()->getFirstVariableIndex();
1156  int dest = mimic_joints_[i]->getFirstVariableIndex();
1157  values[dest] = values[src] * mimic_joints_[i]->getMimicFactor() + mimic_joints_[i]->getMimicOffset();
1158  }
1159 }
1160 
1162 {
1163  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1165  updateMimicJoints(values);
1166 }
1167 
1169  std::map<std::string, double>& values) const
1170 {
1171  std::vector<double> tmp(variable_count_);
1172  getVariableRandomPositions(rng, &tmp[0]);
1173  values.clear();
1174  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1175  values[variable_names_[i]] = tmp[i];
1176 }
1177 
1179 {
1180  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1182  updateMimicJoints(values);
1183 }
1184 
1185 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1186 {
1187  std::vector<double> tmp(variable_count_);
1188  getVariableDefaultPositions(&tmp[0]);
1189  values.clear();
1190  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1191  values[variable_names_[i]] = tmp[i];
1192 }
1193 
1194 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1195  std::vector<std::string>& missing_variables) const
1196 {
1197  missing_variables.clear();
1198  std::set<std::string> keys(variables.begin(), variables.end());
1199  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1200  if (keys.find(variable_names_[i]) == keys.end())
1201  if (getJointOfVariable(variable_names_[i])->getMimic() == nullptr)
1202  missing_variables.push_back(variable_names_[i]);
1203 }
1204 
1205 int RobotModel::getVariableIndex(const std::string& variable) const
1206 {
1207  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1208  if (it == joint_variables_index_map_.end())
1209  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1210  return it->second;
1211 }
1212 
1213 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1214 {
1215  double max_distance = 0.0;
1216  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1217  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1218  active_joint_model_vector_[j]->getDistanceFactor();
1219  return max_distance;
1220 }
1221 
1222 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1223  double margin) const
1224 {
1225  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1226  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1228  *active_joint_bounds[i], margin))
1229  return false;
1230  return true;
1231 }
1232 
1233 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1234 {
1235  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1236  bool change = false;
1237  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1239  *active_joint_bounds[i]))
1240  change = true;
1241  if (change)
1242  updateMimicJoints(state);
1243  return change;
1244 }
1245 
1246 double RobotModel::distance(const double* state1, const double* state2) const
1247 {
1248  double d = 0.0;
1249  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1250  d += active_joint_model_vector_[i]->getDistanceFactor() *
1252  state2 + active_joint_model_start_index_[i]);
1253  return d;
1254 }
1255 
1256 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1257 {
1258  // we interpolate values only for active joint models (non-mimic)
1259  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1262  state + active_joint_model_start_index_[i]);
1263  // now we update mimic as needed
1264  updateMimicJoints(state);
1265 }
1266 
1267 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1268 {
1269  // we first set all the "simple" allocators -- where a group has one IK solver
1271  {
1272  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1273  if (jt != allocators.end())
1274  {
1275  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1276  result.first = jt->second;
1277  jmg->setSolverAllocators(result);
1278  }
1279  }
1280 
1281  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1282  // setSolverAllocators()
1283  for (JointModelGroup* jmg : joint_model_groups_)
1284  {
1285  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1286  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1287  if (jt == allocators.end())
1288  {
1289  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1290  std::set<const JointModel*> joints;
1291  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1292 
1293  std::vector<const JointModelGroup*> subs;
1294 
1295  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1296  for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin(); kt != allocators.end();
1297  ++kt)
1298  {
1299  const JointModelGroup* sub = getJointModelGroup(kt->first);
1300  if (!sub) // this should actually not happen, all groups should be well defined
1301  {
1302  subs.clear();
1303  break;
1304  }
1305  std::set<const JointModel*> sub_joints;
1306  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1307 
1308  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1309  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1310  std::set<const JointModel*> resultj;
1311  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1312  std::inserter(resultj, resultj.end()));
1313  // TODO: instead of maintaining disjoint joint sets here,
1314  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1315  // There, a disjoint bijection from joints to solvers is computed anyway.
1316  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1317  // But, if the overlap only involves fixed joints, we could consider all sub groups
1318  subs.push_back(sub);
1319  joints.swap(resultj);
1320  }
1321  }
1322 
1323  // if we found subgroups, pass that information to the planning group
1324  if (!subs.empty())
1325  {
1326  std::stringstream ss;
1327  for (std::size_t i = 0; i < subs.size(); ++i)
1328  {
1329  ss << subs[i]->getName() << " ";
1330  result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
1331  }
1332  ROS_DEBUG_NAMED("robot_model", "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1333  ss.str().c_str());
1334  }
1335  jmg->setSolverAllocators(result);
1336  }
1337  }
1338 }
1339 
1340 void RobotModel::printModelInfo(std::ostream& out) const
1341 {
1342  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
1343  << std::endl;
1344 
1345  std::ios_base::fmtflags old_flags = out.flags();
1346  out.setf(std::ios::fixed, std::ios::floatfield);
1347  std::streamsize old_prec = out.precision();
1348  out.precision(5);
1349  out << "Joints: " << std::endl;
1350  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
1351  {
1352  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
1353  << std::endl;
1354  out << " * Joint Index: " << joint_model_vector_[i]->getJointIndex() << std::endl;
1355  const std::vector<std::string>& vn = joint_model_vector_[i]->getVariableNames();
1356  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
1357  << std::endl;
1358  int idx = joint_model_vector_[i]->getFirstVariableIndex();
1359  for (std::vector<std::string>::const_iterator it = vn.begin(); it != vn.end(); ++it)
1360  {
1361  out << " * '" << *it << "', index " << idx++ << " in full state";
1362  if (joint_model_vector_[i]->getMimic())
1363  out << ", mimic '" << joint_model_vector_[i]->getMimic()->getName() << "'";
1364  if (joint_model_vector_[i]->isPassive())
1365  out << ", passive";
1366  out << std::endl;
1367  out << " " << joint_model_vector_[i]->getVariableBounds(*it) << std::endl;
1368  }
1369  }
1370  out << std::endl;
1371  out.precision(old_prec);
1372  out.flags(old_flags);
1373  out << "Links: " << std::endl;
1374  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
1375  {
1376  out << " '" << link_model_vector_[i]->getName() << "' with " << link_model_vector_[i]->getShapes().size()
1377  << " geoms" << std::endl;
1378  if (link_model_vector_[i]->parentJointIsFixed())
1379  out << " * "
1380  << "parent joint is fixed" << std::endl;
1381  if (link_model_vector_[i]->jointOriginTransformIsIdentity())
1382  out << " * "
1383  << "joint origin transform is identity" << std::endl;
1384  }
1385 
1386  out << "Available groups: " << std::endl;
1387  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
1388  joint_model_groups_[i]->printGroupInfo(out);
1389 }
1390 
1391 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Affine3d& transform,
1392  LinkTransformMap& associated_transforms)
1393 {
1394  associated_transforms[link] = transform * link->getJointOriginTransform();
1395  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1396  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1397  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1398  transform * link->getJointOriginTransform(), associated_transforms);
1399 }
1400 
1401 } // end of namespace core
1402 } // end of namespace moveit
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:513
d
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
void setChildLinkModel(const LinkModel *link)
Definition: joint_model.h:164
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
std::vector< std::string > subgroups_
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
std::vector< LinkModel * > link_model_vector_
The vector of links that are updated when computeTransforms() is called, in the order they are update...
Definition: robot_model.h:469
JointModel * constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model)
Given a urdf joint model, a child link and a set of virtual joints, build up the corresponding JointM...
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:548
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:475
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...
#define ROS_INFO_NAMED(name,...)
const std::string & getName() const
Get the model name.
Definition: robot_model.h:77
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.
std::vector< const LinkModel * > link_model_vector_const_
The vector of links that are updated when computeTransforms() is called, in the order they are update...
Definition: robot_model.h:472
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
const std::vector< VirtualJoint > & getVirtualJoints() const
#define ROS_WARN_NAMED(name,...)
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:545
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints...
const std::vector< const JointModel * > & getChildJointModels() const
A link may have 0 or more child joints. From those joints there will certainly be other descendant li...
Definition: link_model.h:128
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:466
std::vector< std::pair< std::string, std::string > > chains_
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:199
const std::vector< const JointModel * > & getMimicRequests() const
The joint models whose values would be modified if the value of this joint changed.
Definition: joint_model.h:383
RobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
Construct a kinematic model from a parsed description and a list of planning groups.
Definition: robot_model.cpp:53
void buildJointInfo()
Compute helpful information about joints.
void setJointIndex(int index)
Set the index of this joint within the robot model.
Definition: joint_model.h:220
XmlRpcServer s
void addDescendantJointModel(const JointModel *joint)
const std::string & getName() const
Get the name of the joint group.
void setAxis(const Eigen::Vector3d &axis)
Set the axis of translation.
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:463
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::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:392
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:556
void setJointOriginTransform(const Eigen::Affine3d &transform)
Definition: link_model.cpp:60
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:61
const JointModel * getRootJoint() const
Get the root joint. There will be one root joint unless the model is empty. This is either extracted ...
Definition: robot_model.cpp:71
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
double distance(const double *state1, const double *state2) const
std::shared_ptr< Shape > ShapePtr
__attribute__((deprecated)) std const std::vector< Group > & getGroups() const
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
void setFirstCollisionBodyTransformIndex(int index)
Definition: link_model.h:102
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
void buildGroups(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build up the groups in this kinematic model.
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
void addChildJointModel(const JointModel *joint)
Definition: link_model.h:133
void buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model)
Given an URDF model and a SRDF model, build a full kinematic model.
Definition: robot_model.cpp:81
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:108
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return NULL when the joint is missing.
std::string model_name_
The name of the model.
Definition: robot_model.h:451
LinkModel * constructLinkModel(const urdf::Link *urdf_link)
Given a urdf link, build the corresponding LinkModel object.
#define ROS_DEBUG_NAMED(name,...)
shapes::ShapePtr constructShape(const urdf::Geometry *geom)
Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape ob...
unsigned int index
VariableIndexMap joint_variables_index_map_
The state includes all the joint variables that make up the joints the state consists of...
Definition: robot_model.h:540
const std::vector< const JointModel * > & getFixedJointModels() const
Get the fixed joints that are part of this group.
void computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
Definition: robot_model.h:553
void computeFixedTransforms(const LinkModel *link, const Eigen::Affine3d &transform, LinkTransformMap &associated_transforms)
std::vector< const JointModel::Bounds * > JointBoundsVector
void setAxis(const Eigen::Vector3d &axis)
Set the axis of rotation.
ROSCPP_DECL bool ok()
std::size_t variable_count_
Get the number of variables necessary to describe this model.
Definition: robot_model.h:535
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:489
std::vector< const JointModel * > joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:498
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition: link_model.h:174
std::string name_
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:456
void addDescendantLinkModel(const LinkModel *link)
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:562
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:323
void setLinkIndex(int index)
Definition: link_model.h:92
void buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:510
void setParentLinkModel(const LinkModel *link)
Definition: link_model.h:122
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this joint, in the order they appear in corresponding sta...
Definition: joint_model.h:176
This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of s...
Definition: profiler.h:99
std::vector< const JointModel * > single_dof_joints_
Definition: robot_model.h:515
std::vector< std::string > links_
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:568
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
void computeCommonRoots()
For every pair of joints, pre-compute the common roots of the joints.
std::vector< JointModel * > active_joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:504
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:76
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:484
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:492
Mesh * createMeshFromResource(const std::string &resource)
const std::vector< PassiveJoint > & getPassiveJoints() const
void setGeometry(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &origins)
Definition: link_model.cpp:74
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
Definition: joint_model.h:154
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:501
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:458
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
std::string model_frame_
The reference frame for this model.
Definition: robot_model.h:454
void buildGroupsInfo_Subgroups(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
std::vector< std::string > joints_
#define ROS_ERROR_NAMED(name,...)
std::vector< JointModel * > joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:495
std::vector< std::string > variable_names_
The names of the DOF that make up this state (this is just a sequence of joint variable names; not ne...
Definition: robot_model.h:532
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return a NULL pointer here.
Definition: joint_model.h:148
bool addJointModelGroup(const srdf::Model::Group &group)
Construct a JointModelGroup given a SRDF description group.
void interpolate(const double *from, const double *to, double t, double *state) const
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
const Eigen::Affine3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link&#39;s origin...
Definition: link_model.h:142
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:542
int getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of sc...
Definition: profiler.h:78
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:517
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
Definition: link_model.h:117
void buildMimic(const urdf::ModelInterface &urdf_model)
Given the URDF model, build up the mimic joints (mutually constrained joints)
void setParentJointModel(const JointModel *joint)
Definition: link_model.cpp:68
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:559
const std::vector< const JointModel * > & getJointModels() const
Get the array of joints, in the order they appear in the robot state.
Definition: robot_model.h:142
Main namespace for MoveIt!
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:565
void setParentLinkModel(const LinkModel *link)
Definition: joint_model.h:159
const std::vector< GroupState > & getGroupStates() const
std::vector< const JointModel * > active_joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:507
void buildGroupStates(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build the default states defined in the SRDF...
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:478
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for thw joints is stored.
Definition: robot_model.h:526
JointModel * buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model)
(This function is mostly intended for internal use). Given a parent link, build up (recursively)...
void setVariableBounds(const std::string &variable, const VariableBounds &bounds)
Set the lower and upper bounds for a variable. Throw an exception if the variable was not found...
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:328
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137
void setVisualMesh(const std::string &visual_mesh, const Eigen::Affine3d &origin, const Eigen::Vector3d &scale)
Definition: link_model.cpp:115
double getMaximumExtent() const
Definition: robot_model.h:334
void setPassive(bool flag)
Definition: joint_model.h:417
virtual unsigned int getStateSpaceDimension() const =0
Get the dimension of the state space that corresponds to this joint.
void setDistanceFactor(double factor)
Set the factor that should be applied to the value returned by distance() when that value is used in ...
Definition: joint_model.h:353
const std::vector< EndEffector > & getEndEffectors() const
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
std::vector< std::string > link_model_names_with_collision_geometry_vector_
The vector of link names that corresponds to link_models_with_collision_geometry_vector_.
Definition: robot_model.h:481
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
std::shared_ptr< const Shape > ShapeConstPtr
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 Sat Sep 15 2018 02:51:06