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