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 } // namespace
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(), 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, tf_base.second.inverse() * tf_target.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  ROS_ERROR_NAMED(LOGNAME, "The model for joint '%s' requires %d variable values, "
344  "but only %d variable values were supplied 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  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specifies value for joint '%s', "
350  "but that joint is not part of group '%s'",
351  ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
352  }
353  if (!state.empty())
354  jmg->addDefaultState(ds[i].name_, state);
355  }
356  else
357  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specified for group '%s', but that group does not exist",
358  ds[i].name_.c_str(), ds[i].group_.c_str());
359  }
360 }
361 
362 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
363 {
364  // compute mimic joints
365  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
366  {
367  const urdf::Joint* jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
368  if (jm)
369  if (jm->mimic)
370  {
371  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
372  if (jit != joint_model_map_.end())
373  {
374  if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
375  joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
376  else
377  ROS_ERROR_NAMED(LOGNAME, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
378  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
379  }
380  else
381  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' cannot mimic unknown joint '%s'",
382  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
383  }
384  }
385 
386  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
387  bool change = true;
388  while (change)
389  {
390  change = false;
391  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
392  if (joint_model_vector_[i]->getMimic())
393  {
394  if (joint_model_vector_[i]->getMimic()->getMimic())
395  {
396  joint_model_vector_[i]->setMimic(
397  joint_model_vector_[i]->getMimic()->getMimic(),
398  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicFactor(),
399  joint_model_vector_[i]->getMimicOffset() +
400  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicOffset());
401  change = true;
402  }
403  if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic())
404  {
405  ROS_ERROR_NAMED(LOGNAME, "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
406  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
407  joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0);
408  change = false;
409  break;
410  }
411  }
412  }
413  // build mimic requests
414  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
415  if (joint_model_vector_[i]->getMimic())
416  {
417  const_cast<JointModel*>(joint_model_vector_[i]->getMimic())->addMimicRequest(joint_model_vector_[i]);
418  mimic_joints_.push_back(joint_model_vector_[i]);
419  }
420 }
421 
422 bool RobotModel::hasEndEffector(const std::string& eef) const
423 {
424  return end_effectors_map_.find(eef) != end_effectors_map_.end();
425 }
426 
427 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
428 {
429  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
430  if (it == end_effectors_map_.end())
431  {
432  it = joint_model_group_map_.find(name);
433  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
434  return it->second;
435  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
436  return nullptr;
437  }
438  return it->second;
439 }
440 
442 {
443  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
444  if (it == end_effectors_map_.end())
445  {
446  it = joint_model_group_map_.find(name);
447  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
448  return it->second;
449  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
450  return nullptr;
451  }
452  return it->second;
453 }
454 
455 bool RobotModel::hasJointModelGroup(const std::string& name) const
456 {
457  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
458 }
459 
460 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
461 {
462  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
463  if (it == joint_model_group_map_.end())
464  {
465  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
466  return nullptr;
467  }
468  return it->second;
469 }
470 
472 {
473  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
474  if (it == joint_model_group_map_.end())
475  {
476  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
477  return nullptr;
478  }
479  return it->second;
480 }
481 
482 void RobotModel::buildGroups(const srdf::Model& srdf_model)
483 {
484  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
485 
486  // the only thing tricky is dealing with subgroups
487  std::vector<bool> processed(group_configs.size(), false);
488 
489  bool added = true;
490  while (added)
491  {
492  added = false;
493 
494  // going to make passes until we can't do anything else
495  for (std::size_t i = 0; i < group_configs.size(); ++i)
496  if (!processed[i])
497  {
498  // if we haven't processed, check and see if the dependencies are met yet
499  bool all_subgroups_added = true;
500  for (std::size_t j = 0; j < group_configs[i].subgroups_.size(); ++j)
501  if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
502  {
503  all_subgroups_added = false;
504  break;
505  }
506  if (all_subgroups_added)
507  {
508  added = true;
509  processed[i] = true;
510  if (!addJointModelGroup(group_configs[i]))
511  ROS_WARN_NAMED(LOGNAME, "Failed to add group '%s'", group_configs[i].name_.c_str());
512  }
513  }
514  }
515 
516  for (std::size_t i = 0; i < processed.size(); ++i)
517  if (!processed[i])
518  ROS_WARN_NAMED(LOGNAME, "Could not process group '%s' due to unmet subgroup dependencies",
519  group_configs[i].name_.c_str());
520 
521  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
522  joint_model_groups_.push_back(it->second);
523  std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
524  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
525  {
528  }
529 
530  buildGroupsInfoSubgroups(srdf_model);
531  buildGroupsInfoEndEffectors(srdf_model);
532 }
533 
535 {
536  // compute subgroups
537  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
538  {
539  JointModelGroup* jmg = it->second;
540  std::vector<std::string> subgroup_names;
541  std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
542  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
543  ++jt)
544  if (jt->first != it->first)
545  {
546  bool ok = true;
547  JointModelGroup* sub_jmg = jt->second;
548  const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
549  for (std::size_t k = 0; k < sub_joints.size(); ++k)
550  if (joints.find(sub_joints[k]) == joints.end())
551  {
552  ok = false;
553  break;
554  }
555  if (ok)
556  subgroup_names.push_back(sub_jmg->getName());
557  }
558  if (!subgroup_names.empty())
559  jmg->setSubgroupNames(subgroup_names);
560  }
561 }
562 
564 {
565  // set the end-effector flags
566  const std::vector<srdf::Model::EndEffector>& eefs = srdf_model.getEndEffectors();
567  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
568  {
569  // check if this group is a known end effector
570  for (std::size_t k = 0; k < eefs.size(); ++k)
571  if (eefs[k].component_group_ == it->first)
572  {
573  // if it is, mark it as such
574  it->second->setEndEffectorName(eefs[k].name_);
575  end_effectors_map_[eefs[k].name_] = it->second;
576  end_effectors_.push_back(it->second);
577 
578  // check to see if there are groups that contain the parent link of this end effector.
579  // record this information if found;
580  std::vector<JointModelGroup*> possible_parent_groups;
581  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
582  ++jt)
583  if (jt->first != it->first)
584  {
585  if (jt->second->hasLinkModel(eefs[k].parent_link_))
586  {
587  jt->second->attachEndEffector(eefs[k].name_);
588  possible_parent_groups.push_back(jt->second);
589  }
590  }
591 
592  JointModelGroup* eef_parent_group = nullptr;
593  // if a parent group is specified in SRDF, try to use it
594  if (!eefs[k].parent_group_.empty())
595  {
596  JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
597  if (jt != joint_model_group_map_.end())
598  {
599  if (jt->second->hasLinkModel(eefs[k].parent_link_))
600  {
601  if (jt->second != it->second)
602  eef_parent_group = jt->second;
603  else
604  ROS_ERROR_NAMED(LOGNAME, "Group '%s' for end-effector '%s' cannot be its own parent",
605  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
606  }
607  else
608  ROS_ERROR_NAMED(LOGNAME, "Group '%s' was specified as parent group for end-effector '%s' "
609  "but it does not include the parent link '%s'",
610  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
611  }
612  else
613  ROS_ERROR_NAMED(LOGNAME, "Group name '%s' not found (specified as parent group for end-effector '%s')",
614  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
615  }
616 
617  // if no parent group was specified, use a default one
618  if (eef_parent_group == nullptr)
619  if (!possible_parent_groups.empty())
620  {
621  // if there are multiple options for the group that contains this end-effector,
622  // we pick the group with fewest joints.
623  std::size_t best = 0;
624  for (std::size_t g = 1; g < possible_parent_groups.size(); ++g)
625  if (possible_parent_groups[g]->getJointModels().size() <
626  possible_parent_groups[best]->getJointModels().size())
627  best = g;
628  eef_parent_group = possible_parent_groups[best];
629  }
630 
631  if (eef_parent_group)
632  {
633  it->second->setEndEffectorParent(eef_parent_group->getName(), eefs[k].parent_link_);
634  }
635  else
636  {
637  ROS_WARN_NAMED(LOGNAME, "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
638  it->second->setEndEffectorParent("", eefs[k].parent_link_);
639  }
640  }
641  }
642  std::sort(end_effectors_.begin(), end_effectors_.end(), OrderGroupsByName());
643 }
644 
646 {
648  {
649  ROS_WARN_NAMED(LOGNAME, "A group named '%s' already exists. Not adding.", gc.name_.c_str());
650  return false;
651  }
652 
653  std::set<const JointModel*> jset;
654 
655  // add joints from chains
656  for (std::size_t i = 0; i < gc.chains_.size(); ++i)
657  {
658  const LinkModel* base_link = getLinkModel(gc.chains_[i].first);
659  const LinkModel* tip_link = getLinkModel(gc.chains_[i].second);
660  if (base_link && tip_link)
661  {
662  // go from tip, up the chain, until we hit the root or we find the base_link
663  const LinkModel* lm = tip_link;
664  std::vector<const JointModel*> cj;
665  while (lm)
666  {
667  if (lm == base_link)
668  break;
669  cj.push_back(lm->getParentJointModel());
671  }
672  // if we did not find the base_link, we could have a chain like e.g.,
673  // from one end-effector to another end-effector, so the root is in between
674  if (lm != base_link)
675  {
676  // we go up the chain from the base this time, and see where we intersect the other chain
677  lm = base_link;
678  std::size_t index = 0;
679  std::vector<const JointModel*> cj2;
680  while (lm)
681  {
682  for (std::size_t j = 0; j < cj.size(); ++j)
683  if (cj[j] == lm->getParentJointModel())
684  {
685  index = j + 1;
686  break;
687  }
688  if (index > 0)
689  break;
690  cj2.push_back(lm->getParentJointModel());
692  }
693  if (index > 0)
694  {
695  jset.insert(cj.begin(), cj.begin() + index);
696  jset.insert(cj2.begin(), cj2.end());
697  }
698  }
699  else
700  // if we have a simple chain, just add the joints
701  jset.insert(cj.begin(), cj.end());
702  }
703  }
704 
705  // add joints
706  for (std::size_t i = 0; i < gc.joints_.size(); ++i)
707  {
708  const JointModel* j = getJointModel(gc.joints_[i]);
709  if (j)
710  jset.insert(j);
711  }
712 
713  // add joints that are parents of included links
714  for (std::size_t i = 0; i < gc.links_.size(); ++i)
715  {
716  const LinkModel* l = getLinkModel(gc.links_[i]);
717  if (l)
718  jset.insert(l->getParentJointModel());
719  }
720 
721  // add joints from subgroups
722  for (std::size_t i = 0; i < gc.subgroups_.size(); ++i)
723  {
724  const JointModelGroup* sg = getJointModelGroup(gc.subgroups_[i]);
725  if (sg)
726  {
727  // active joints
728  const std::vector<const JointModel*>& js = sg->getJointModels();
729  for (std::size_t j = 0; j < js.size(); ++j)
730  jset.insert(js[j]);
731 
732  // fixed joints
733  const std::vector<const JointModel*>& fs = sg->getFixedJointModels();
734  for (std::size_t j = 0; j < fs.size(); ++j)
735  jset.insert(fs[j]);
736 
737  // mimic joints
738  const std::vector<const JointModel*>& ms = sg->getMimicJointModels();
739  for (std::size_t j = 0; j < ms.size(); ++j)
740  jset.insert(ms[j]);
741  }
742  }
743 
744  if (jset.empty())
745  {
746  ROS_WARN_NAMED(LOGNAME, "Group '%s' must have at least one valid joint", gc.name_.c_str());
747  return false;
748  }
749 
750  std::vector<const JointModel*> joints;
751  for (std::set<const JointModel*>::iterator it = jset.begin(); it != jset.end(); ++it)
752  joints.push_back(*it);
753 
754  JointModelGroup* jmg = new JointModelGroup(gc.name_, gc, joints, this);
755  joint_model_group_map_[gc.name_] = jmg;
756 
757  return true;
758 }
759 
760 JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, 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(nullptr, urdf_link, srdf_model);
766  if (joint == nullptr)
767  return nullptr;
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 VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
811 {
812  VariableBounds b;
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 } // namespace
844 
845 JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link,
846  const srdf::Model& srdf_model)
847 {
848  JointModel* result = nullptr;
849 
850  // if urdf_joint exists, must be the root link transform
851  if (urdf_joint)
852  {
853  switch (urdf_joint->type)
854  {
855  case urdf::Joint::REVOLUTE:
856  {
857  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
858  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
859  j->setContinuous(false);
860  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
861  result = j;
862  }
863  break;
864  case urdf::Joint::CONTINUOUS:
865  {
866  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
867  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
868  j->setContinuous(true);
869  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
870  result = j;
871  }
872  break;
873  case urdf::Joint::PRISMATIC:
874  {
875  PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name);
876  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
877  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
878  result = j;
879  }
880  break;
881  case urdf::Joint::FLOATING:
882  result = new FloatingJointModel(urdf_joint->name);
883  break;
884  case urdf::Joint::PLANAR:
885  result = new PlanarJointModel(urdf_joint->name);
886  break;
887  case urdf::Joint::FIXED:
888  result = new FixedJointModel(urdf_joint->name);
889  break;
890  default:
891  ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type);
892  break;
893  }
894  }
895  else // if urdf_joint passed in as null, then we're at root of URDF model
896  {
897  const std::vector<srdf::Model::VirtualJoint>& virtual_joints = srdf_model.getVirtualJoints();
898  for (std::size_t i = 0; i < virtual_joints.size(); ++i)
899  {
900  if (virtual_joints[i].child_link_ != child_link->name)
901  {
902  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its child frame '%s' "
903  "does not match the URDF frame '%s'",
904  virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(),
905  child_link->name.c_str());
906  }
907  else if (virtual_joints[i].parent_frame_.empty())
908  {
909  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its parent frame is empty",
910  virtual_joints[i].name_.c_str());
911  }
912  else
913  {
914  if (virtual_joints[i].type_ == "fixed")
915  result = new FixedJointModel(virtual_joints[i].name_);
916  else if (virtual_joints[i].type_ == "planar")
917  result = new PlanarJointModel(virtual_joints[i].name_);
918  else if (virtual_joints[i].type_ == "floating")
919  result = new FloatingJointModel(virtual_joints[i].name_);
920  if (result)
921  {
922  // for fixed frames we still use the robot root link
923  if (virtual_joints[i].type_ != "fixed")
924  {
925  model_frame_ = virtual_joints[i].parent_frame_;
926  }
927  break;
928  }
929  }
930  }
931  if (!result)
932  {
933  ROS_INFO_NAMED(LOGNAME, "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::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose)
958 {
959  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
960  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
961  return af;
962 }
963 }
964 
965 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
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(urdfPose2Isometry3d(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(urdfPose2Isometry3d(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, urdfPose2Isometry3d(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, urdfPose2Isometry3d(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(urdfPose2Isometry3d(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 = nullptr;
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  ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type);
1066  break;
1067  }
1068 
1069  return shapes::ShapePtr(result);
1070 }
1071 
1072 bool RobotModel::hasJointModel(const std::string& name) const
1073 {
1074  return joint_model_map_.find(name) != joint_model_map_.end();
1075 }
1076 
1077 bool RobotModel::hasLinkModel(const std::string& name) const
1078 {
1079  return link_model_map_.find(name) != link_model_map_.end();
1080 }
1081 
1082 const JointModel* RobotModel::getJointModel(const std::string& name) const
1083 {
1084  JointModelMap::const_iterator it = joint_model_map_.find(name);
1085  if (it != joint_model_map_.end())
1086  return it->second;
1087  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1088  return nullptr;
1089 }
1090 
1091 const JointModel* RobotModel::getJointModel(int index) const
1092 {
1093  if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
1094  {
1095  ROS_ERROR_NAMED(LOGNAME, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
1096  return nullptr;
1097  }
1098  assert(joint_model_vector_[index]->getJointIndex() == index);
1099  return joint_model_vector_[index];
1100 }
1101 
1102 JointModel* RobotModel::getJointModel(const std::string& name)
1103 {
1104  JointModelMap::const_iterator it = joint_model_map_.find(name);
1105  if (it != joint_model_map_.end())
1106  return it->second;
1107  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1108  return nullptr;
1109 }
1110 
1111 const LinkModel* RobotModel::getLinkModel(const std::string& name) const
1112 {
1113  return const_cast<RobotModel*>(this)->getLinkModel(name);
1114 }
1115 
1116 const LinkModel* RobotModel::getLinkModel(int index) const
1117 {
1118  if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
1119  {
1120  ROS_ERROR_NAMED(LOGNAME, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
1121  return nullptr;
1122  }
1123  assert(link_model_vector_[index]->getLinkIndex() == index);
1124  return link_model_vector_[index];
1125 }
1126 
1127 LinkModel* RobotModel::getLinkModel(const std::string& name)
1128 {
1129  LinkModelMap::const_iterator it = link_model_map_.find(name);
1130  if (it != link_model_map_.end())
1131  return it->second;
1132  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1133  return nullptr;
1134 }
1135 
1137 {
1138  if (!link)
1139  return link;
1140  const robot_model::LinkModel* parent_link = link->getParentLinkModel();
1141  const robot_model::JointModel* joint = link->getParentJointModel();
1142 
1143  while (parent_link && joint->getType() == robot_model::JointModel::FIXED)
1144  {
1145  link = parent_link;
1146  joint = link->getParentJointModel();
1147  parent_link = joint->getParentLinkModel();
1148  }
1149  return link;
1150 }
1151 
1152 void RobotModel::updateMimicJoints(double* values) const
1153 {
1154  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
1155  {
1156  int src = mimic_joints_[i]->getMimic()->getFirstVariableIndex();
1157  int dest = mimic_joints_[i]->getFirstVariableIndex();
1158  values[dest] = values[src] * mimic_joints_[i]->getMimicFactor() + mimic_joints_[i]->getMimicOffset();
1159  }
1160 }
1161 
1163 {
1164  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1166  updateMimicJoints(values);
1167 }
1168 
1170  std::map<std::string, double>& values) const
1171 {
1172  std::vector<double> tmp(variable_count_);
1173  getVariableRandomPositions(rng, &tmp[0]);
1174  values.clear();
1175  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1176  values[variable_names_[i]] = tmp[i];
1177 }
1178 
1180 {
1181  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1183  updateMimicJoints(values);
1184 }
1185 
1186 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1187 {
1188  std::vector<double> tmp(variable_count_);
1189  getVariableDefaultPositions(&tmp[0]);
1190  values.clear();
1191  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1192  values[variable_names_[i]] = tmp[i];
1193 }
1194 
1195 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1196  std::vector<std::string>& missing_variables) const
1197 {
1198  missing_variables.clear();
1199  std::set<std::string> keys(variables.begin(), variables.end());
1200  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1201  if (keys.find(variable_names_[i]) == keys.end())
1202  if (getJointOfVariable(variable_names_[i])->getMimic() == nullptr)
1203  missing_variables.push_back(variable_names_[i]);
1204 }
1205 
1206 int RobotModel::getVariableIndex(const std::string& variable) const
1207 {
1208  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1209  if (it == joint_variables_index_map_.end())
1210  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1211  return it->second;
1212 }
1213 
1214 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1215 {
1216  double max_distance = 0.0;
1217  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1218  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1219  active_joint_model_vector_[j]->getDistanceFactor();
1220  return max_distance;
1221 }
1222 
1223 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1224  double margin) const
1225 {
1226  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1227  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1229  *active_joint_bounds[i], margin))
1230  return false;
1231  return true;
1232 }
1233 
1234 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1235 {
1236  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1237  bool change = false;
1238  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1240  *active_joint_bounds[i]))
1241  change = true;
1242  if (change)
1243  updateMimicJoints(state);
1244  return change;
1245 }
1246 
1247 double RobotModel::distance(const double* state1, const double* state2) const
1248 {
1249  double d = 0.0;
1250  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1251  d += active_joint_model_vector_[i]->getDistanceFactor() *
1253  state2 + active_joint_model_start_index_[i]);
1254  return d;
1255 }
1256 
1257 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1258 {
1259  // we interpolate values only for active joint models (non-mimic)
1260  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1263  state + active_joint_model_start_index_[i]);
1264  // now we update mimic as needed
1265  updateMimicJoints(state);
1266 }
1267 
1268 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1269 {
1270  // we first set all the "simple" allocators -- where a group has one IK solver
1272  {
1273  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1274  if (jt != allocators.end())
1275  {
1276  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1277  result.first = jt->second;
1278  jmg->setSolverAllocators(result);
1279  }
1280  }
1281 
1282  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1283  // setSolverAllocators()
1284  for (JointModelGroup* jmg : joint_model_groups_)
1285  {
1286  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1287  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1288  if (jt == allocators.end())
1289  {
1290  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1291  std::set<const JointModel*> joints;
1292  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1293 
1294  std::vector<const JointModelGroup*> subs;
1295 
1296  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1297  for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin(); kt != allocators.end();
1298  ++kt)
1299  {
1300  const JointModelGroup* sub = getJointModelGroup(kt->first);
1301  if (!sub) // this should actually not happen, all groups should be well defined
1302  {
1303  subs.clear();
1304  break;
1305  }
1306  std::set<const JointModel*> sub_joints;
1307  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1308 
1309  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1310  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1311  std::set<const JointModel*> resultj;
1312  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1313  std::inserter(resultj, resultj.end()));
1314  // TODO: instead of maintaining disjoint joint sets here,
1315  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1316  // There, a disjoint bijection from joints to solvers is computed anyway.
1317  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1318  // But, if the overlap only involves fixed joints, we could consider all sub groups
1319  subs.push_back(sub);
1320  joints.swap(resultj);
1321  }
1322  }
1323 
1324  // if we found subgroups, pass that information to the planning group
1325  if (!subs.empty())
1326  {
1327  std::stringstream ss;
1328  for (std::size_t i = 0; i < subs.size(); ++i)
1329  {
1330  ss << subs[i]->getName() << " ";
1331  result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
1332  }
1333  ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1334  ss.str().c_str());
1335  }
1336  jmg->setSolverAllocators(result);
1337  }
1338  }
1339 }
1340 
1341 void RobotModel::printModelInfo(std::ostream& out) const
1342 {
1343  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
1344  << std::endl;
1345 
1346  std::ios_base::fmtflags old_flags = out.flags();
1347  out.setf(std::ios::fixed, std::ios::floatfield);
1348  std::streamsize old_prec = out.precision();
1349  out.precision(5);
1350  out << "Joints: " << std::endl;
1351  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
1352  {
1353  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
1354  << std::endl;
1355  out << " * Joint Index: " << joint_model_vector_[i]->getJointIndex() << std::endl;
1356  const std::vector<std::string>& vn = joint_model_vector_[i]->getVariableNames();
1357  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
1358  << std::endl;
1359  int idx = joint_model_vector_[i]->getFirstVariableIndex();
1360  for (std::vector<std::string>::const_iterator it = vn.begin(); it != vn.end(); ++it)
1361  {
1362  out << " * '" << *it << "', index " << idx++ << " in full state";
1363  if (joint_model_vector_[i]->getMimic())
1364  out << ", mimic '" << joint_model_vector_[i]->getMimic()->getName() << "'";
1365  if (joint_model_vector_[i]->isPassive())
1366  out << ", passive";
1367  out << std::endl;
1368  out << " " << joint_model_vector_[i]->getVariableBounds(*it) << std::endl;
1369  }
1370  }
1371  out << std::endl;
1372  out.precision(old_prec);
1373  out.flags(old_flags);
1374  out << "Links: " << std::endl;
1375  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
1376  {
1377  out << " '" << link_model_vector_[i]->getName() << "' with " << link_model_vector_[i]->getShapes().size()
1378  << " geoms" << std::endl;
1379  if (link_model_vector_[i]->parentJointIsFixed())
1380  out << " * "
1381  << "parent joint is fixed" << std::endl;
1382  if (link_model_vector_[i]->jointOriginTransformIsIdentity())
1383  out << " * "
1384  << "joint origin transform is identity" << std::endl;
1385  }
1386 
1387  out << "Available groups: " << std::endl;
1388  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
1389  joint_model_groups_[i]->printGroupInfo(out);
1390 }
1391 
1392 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
1393  LinkTransformMap& associated_transforms)
1394 {
1395  associated_transforms[link] = transform * link->getJointOriginTransform();
1396  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1397  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1398  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1399  transform * link->getJointOriginTransform(), associated_transforms);
1400 }
1401 
1402 } // end of namespace core
1403 } // end of namespace moveit
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:515
void setVisualMesh(const std::string &visual_mesh, const Eigen::Isometry3d &origin, const Eigen::Vector3d &scale)
Definition: link_model.cpp:116
d
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
void setChildLinkModel(const LinkModel *link)
Definition: joint_model.h:164
std::vector< std::string > subgroups_
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:328
const std::vector< PassiveJoint > & getPassiveJoints() const
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition: link_model.h:174
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:471
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:550
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:477
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...
void computeFixedTransforms(const LinkModel *link, const Eigen::Isometry3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
#define ROS_INFO_NAMED(name,...)
const Eigen::Isometry3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link&#39;s origin...
Definition: link_model.h:142
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:199
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:474
void buildGroupsInfoSubgroups(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
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:391
#define ROS_WARN_NAMED(name,...)
const std::vector< VirtualJoint > & getVirtualJoints() const
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:547
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints...
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:468
std::vector< std::pair< std::string, std::string > > chains_
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
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
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 buildGroupsInfoEndEffectors(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
void setJointOriginTransform(const Eigen::Isometry3d &transform)
Definition: link_model.cpp:60
void setJointIndex(int index)
Set the index of this joint within the robot model.
Definition: joint_model.h:220
XmlRpcServer s
int getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
void addDescendantJointModel(const JointModel *joint)
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:465
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:558
double distance(const double *state1, const double *state2) const
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:63
std::shared_ptr< Shape > ShapePtr
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
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 buildGroups(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build up the groups in this kinematic model.
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
void 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
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:78
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
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
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
std::string model_name_
The name of the robot.
Definition: robot_model.h:452
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
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:542
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
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 computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
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::shared_ptr< const Model > ModelConstSharedPtr
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:555
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
std::vector< const JointModel::Bounds * > JointBoundsVector
const std::vector< const JointModel * > & getFixedJointModels() const
Get the fixed joints that are part of this group.
const std::string & getName() const
Get the name of the joint group.
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:537
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:491
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:500
void setGeometry(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &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::string name_
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
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:458
void addDescendantLinkModel(const LinkModel *link)
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:564
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
void setLinkIndex(int index)
Definition: link_model.h:92
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:512
void setParentLinkModel(const LinkModel *link)
Definition: link_model.h:122
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:517
std::vector< std::string > links_
const std::string LOGNAME
Definition: robot_model.cpp:53
const std::vector< Group > & getGroups() const
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:570
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
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:506
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:486
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:494
Mesh * createMeshFromResource(const std::string &resource)
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:503
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:460
const std::vector< GroupState > & getGroupStates() const
std::string model_frame_
The reference (base) frame for this model. The frame is either extracted from the SRDF as a virtual j...
Definition: robot_model.h:456
std::vector< std::string > joints_
#define ROS_ERROR_NAMED(name,...)
void interpolate(const double *from, const double *to, double t, double *state) const
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:497
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:534
const std::vector< EndEffector > & getEndEffectors() const
bool addJointModelGroup(const srdf::Model::Group &group)
Construct a JointModelGroup given a SRDF description group.
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:544
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of sc...
Definition: profiler.h:78
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:519
std::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:392
void buildMimic(const urdf::ModelInterface &urdf_model)
Given the URDF model, build up the mimic joints (mutually constrained joints)
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:323
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
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:561
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:567
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
void setParentLinkModel(const LinkModel *link)
Definition: joint_model.h:159
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:509
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:480
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:528
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...
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
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137
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
void setPassive(bool flag)
Definition: joint_model.h:425
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:361
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:483
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:370
double getMaximumExtent() const
Definition: robot_model.h:334
const std::string & getName() const
Get the model name.
Definition: robot_model.h:77
std::shared_ptr< const Shape > ShapeConstPtr
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Feb 17 2020 03:50:49