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