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::vector<const JointModel*> remaining_joints = jmg->getActiveJointModels();
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  // Remove current joint name from remaining list.
341  auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm);
342  if (it_found != remaining_joints.end())
343  remaining_joints.erase(it_found);
344  if (vn.size() == jt->second.size())
345  for (std::size_t j = 0; j < vn.size(); ++j)
346  state[vn[j]] = jt->second[j];
347  else
348  ROS_ERROR_NAMED(LOGNAME, "The model for joint '%s' requires %d variable values, "
349  "but only %d variable values were supplied in default state '%s' for group '%s'",
350  jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(),
351  jmg->getName().c_str());
352  }
353  else
354  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specifies value for joint '%s', "
355  "but that joint is not part of group '%s'",
356  ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
357  }
358  if (!remaining_joints.empty())
359  {
360  std::stringstream missing;
361  missing << (*remaining_joints.begin())->getName();
362  for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); j++)
363  {
364  missing << ", " << (*j)->getName();
365  }
366  ROS_WARN_STREAM_NAMED(LOGNAME, "Group state '" << ds[i].name_ << "' doesn't specify all group joints in group '"
367  << ds[i].group_ << "'. " << missing.str() << " "
368  << (remaining_joints.size() > 1 ? "are" : "is") << " missing.");
369  }
370  if (!state.empty())
371  jmg->addDefaultState(ds[i].name_, state);
372  }
373  else
374  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specified for group '%s', but that group does not exist",
375  ds[i].name_.c_str(), ds[i].group_.c_str());
376  }
377 }
378 
379 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
380 {
381  // compute mimic joints
382  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
383  {
384  const urdf::Joint* jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
385  if (jm)
386  if (jm->mimic)
387  {
388  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
389  if (jit != joint_model_map_.end())
390  {
391  if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
392  joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
393  else
394  ROS_ERROR_NAMED(LOGNAME, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
395  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
396  }
397  else
398  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' cannot mimic unknown joint '%s'",
399  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
400  }
401  }
402 
403  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
404  bool change = true;
405  while (change)
406  {
407  change = false;
408  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
409  if (joint_model_vector_[i]->getMimic())
410  {
411  if (joint_model_vector_[i]->getMimic()->getMimic())
412  {
413  joint_model_vector_[i]->setMimic(
414  joint_model_vector_[i]->getMimic()->getMimic(),
415  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicFactor(),
416  joint_model_vector_[i]->getMimicOffset() +
417  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicOffset());
418  change = true;
419  }
420  if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic())
421  {
422  ROS_ERROR_NAMED(LOGNAME, "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
423  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
424  joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0);
425  change = false;
426  break;
427  }
428  }
429  }
430  // build mimic requests
431  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
432  if (joint_model_vector_[i]->getMimic())
433  {
434  const_cast<JointModel*>(joint_model_vector_[i]->getMimic())->addMimicRequest(joint_model_vector_[i]);
435  mimic_joints_.push_back(joint_model_vector_[i]);
436  }
437 }
438 
439 bool RobotModel::hasEndEffector(const std::string& eef) const
440 {
441  return end_effectors_map_.find(eef) != end_effectors_map_.end();
442 }
443 
444 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
445 {
446  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
447  if (it == end_effectors_map_.end())
448  {
449  it = joint_model_group_map_.find(name);
450  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
451  return it->second;
452  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
453  return nullptr;
454  }
455  return it->second;
456 }
457 
459 {
460  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
461  if (it == end_effectors_map_.end())
462  {
463  it = joint_model_group_map_.find(name);
464  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
465  return it->second;
466  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
467  return nullptr;
468  }
469  return it->second;
470 }
471 
472 bool RobotModel::hasJointModelGroup(const std::string& name) const
473 {
474  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
475 }
476 
477 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
478 {
479  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
480  if (it == joint_model_group_map_.end())
481  {
482  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
483  return nullptr;
484  }
485  return it->second;
486 }
487 
489 {
490  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
491  if (it == joint_model_group_map_.end())
492  {
493  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
494  return nullptr;
495  }
496  return it->second;
497 }
498 
499 void RobotModel::buildGroups(const srdf::Model& srdf_model)
500 {
501  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
502 
503  // the only thing tricky is dealing with subgroups
504  std::vector<bool> processed(group_configs.size(), false);
505 
506  bool added = true;
507  while (added)
508  {
509  added = false;
510 
511  // going to make passes until we can't do anything else
512  for (std::size_t i = 0; i < group_configs.size(); ++i)
513  if (!processed[i])
514  {
515  // if we haven't processed, check and see if the dependencies are met yet
516  bool all_subgroups_added = true;
517  for (std::size_t j = 0; j < group_configs[i].subgroups_.size(); ++j)
518  if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
519  {
520  all_subgroups_added = false;
521  break;
522  }
523  if (all_subgroups_added)
524  {
525  added = true;
526  processed[i] = true;
527  if (!addJointModelGroup(group_configs[i]))
528  ROS_WARN_NAMED(LOGNAME, "Failed to add group '%s'", group_configs[i].name_.c_str());
529  }
530  }
531  }
532 
533  for (std::size_t i = 0; i < processed.size(); ++i)
534  if (!processed[i])
535  ROS_WARN_NAMED(LOGNAME, "Could not process group '%s' due to unmet subgroup dependencies",
536  group_configs[i].name_.c_str());
537 
538  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
539  joint_model_groups_.push_back(it->second);
540  std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
541  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
542  {
545  }
546 
547  buildGroupsInfoSubgroups(srdf_model);
548  buildGroupsInfoEndEffectors(srdf_model);
549 }
550 
552 {
553  // compute subgroups
554  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
555  {
556  JointModelGroup* jmg = it->second;
557  std::vector<std::string> subgroup_names;
558  std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
559  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
560  ++jt)
561  if (jt->first != it->first)
562  {
563  bool ok = true;
564  JointModelGroup* sub_jmg = jt->second;
565  const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
566  for (std::size_t k = 0; k < sub_joints.size(); ++k)
567  if (joints.find(sub_joints[k]) == joints.end())
568  {
569  ok = false;
570  break;
571  }
572  if (ok)
573  subgroup_names.push_back(sub_jmg->getName());
574  }
575  if (!subgroup_names.empty())
576  jmg->setSubgroupNames(subgroup_names);
577  }
578 }
579 
581 {
582  // set the end-effector flags
583  const std::vector<srdf::Model::EndEffector>& eefs = srdf_model.getEndEffectors();
584  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
585  {
586  // check if this group is a known end effector
587  for (std::size_t k = 0; k < eefs.size(); ++k)
588  if (eefs[k].component_group_ == it->first)
589  {
590  // if it is, mark it as such
591  it->second->setEndEffectorName(eefs[k].name_);
592  end_effectors_map_[eefs[k].name_] = it->second;
593  end_effectors_.push_back(it->second);
594 
595  // check to see if there are groups that contain the parent link of this end effector.
596  // record this information if found;
597  std::vector<JointModelGroup*> possible_parent_groups;
598  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
599  ++jt)
600  if (jt->first != it->first)
601  {
602  if (jt->second->hasLinkModel(eefs[k].parent_link_))
603  {
604  jt->second->attachEndEffector(eefs[k].name_);
605  possible_parent_groups.push_back(jt->second);
606  }
607  }
608 
609  JointModelGroup* eef_parent_group = nullptr;
610  // if a parent group is specified in SRDF, try to use it
611  if (!eefs[k].parent_group_.empty())
612  {
613  JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
614  if (jt != joint_model_group_map_.end())
615  {
616  if (jt->second->hasLinkModel(eefs[k].parent_link_))
617  {
618  if (jt->second != it->second)
619  eef_parent_group = jt->second;
620  else
621  ROS_ERROR_NAMED(LOGNAME, "Group '%s' for end-effector '%s' cannot be its own parent",
622  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
623  }
624  else
625  ROS_ERROR_NAMED(LOGNAME, "Group '%s' was specified as parent group for end-effector '%s' "
626  "but it does not include the parent link '%s'",
627  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str(), eefs[k].parent_link_.c_str());
628  }
629  else
630  ROS_ERROR_NAMED(LOGNAME, "Group name '%s' not found (specified as parent group for end-effector '%s')",
631  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
632  }
633 
634  // if no parent group was specified, use a default one
635  if (eef_parent_group == nullptr)
636  if (!possible_parent_groups.empty())
637  {
638  // if there are multiple options for the group that contains this end-effector,
639  // we pick the group with fewest joints.
640  std::size_t best = 0;
641  for (std::size_t g = 1; g < possible_parent_groups.size(); ++g)
642  if (possible_parent_groups[g]->getJointModels().size() <
643  possible_parent_groups[best]->getJointModels().size())
644  best = g;
645  eef_parent_group = possible_parent_groups[best];
646  }
647 
648  if (eef_parent_group)
649  {
650  it->second->setEndEffectorParent(eef_parent_group->getName(), eefs[k].parent_link_);
651  }
652  else
653  {
654  ROS_WARN_NAMED(LOGNAME, "Could not identify parent group for end-effector '%s'", eefs[k].name_.c_str());
655  it->second->setEndEffectorParent("", eefs[k].parent_link_);
656  }
657  }
658  }
659  std::sort(end_effectors_.begin(), end_effectors_.end(), OrderGroupsByName());
660 }
661 
663 {
665  {
666  ROS_WARN_NAMED(LOGNAME, "A group named '%s' already exists. Not adding.", gc.name_.c_str());
667  return false;
668  }
669 
670  std::set<const JointModel*> jset;
671 
672  // add joints from chains
673  for (std::size_t i = 0; i < gc.chains_.size(); ++i)
674  {
675  const LinkModel* base_link = getLinkModel(gc.chains_[i].first);
676  const LinkModel* tip_link = getLinkModel(gc.chains_[i].second);
677  if (base_link && tip_link)
678  {
679  // go from tip, up the chain, until we hit the root or we find the base_link
680  const LinkModel* lm = tip_link;
681  std::vector<const JointModel*> cj;
682  while (lm)
683  {
684  if (lm == base_link)
685  break;
686  cj.push_back(lm->getParentJointModel());
688  }
689  // if we did not find the base_link, we could have a chain like e.g.,
690  // from one end-effector to another end-effector, so the root is in between
691  if (lm != base_link)
692  {
693  // we go up the chain from the base this time, and see where we intersect the other chain
694  lm = base_link;
695  std::size_t index = 0;
696  std::vector<const JointModel*> cj2;
697  while (lm)
698  {
699  for (std::size_t j = 0; j < cj.size(); ++j)
700  if (cj[j] == lm->getParentJointModel())
701  {
702  index = j + 1;
703  break;
704  }
705  if (index > 0)
706  break;
707  cj2.push_back(lm->getParentJointModel());
709  }
710  if (index > 0)
711  {
712  jset.insert(cj.begin(), cj.begin() + index);
713  jset.insert(cj2.begin(), cj2.end());
714  }
715  }
716  else
717  // if we have a simple chain, just add the joints
718  jset.insert(cj.begin(), cj.end());
719  }
720  }
721 
722  // add joints
723  for (std::size_t i = 0; i < gc.joints_.size(); ++i)
724  {
725  const JointModel* j = getJointModel(gc.joints_[i]);
726  if (j)
727  jset.insert(j);
728  }
729 
730  // add joints that are parents of included links
731  for (std::size_t i = 0; i < gc.links_.size(); ++i)
732  {
733  const LinkModel* l = getLinkModel(gc.links_[i]);
734  if (l)
735  jset.insert(l->getParentJointModel());
736  }
737 
738  // add joints from subgroups
739  for (std::size_t i = 0; i < gc.subgroups_.size(); ++i)
740  {
741  const JointModelGroup* sg = getJointModelGroup(gc.subgroups_[i]);
742  if (sg)
743  {
744  // active joints
745  const std::vector<const JointModel*>& js = sg->getJointModels();
746  for (std::size_t j = 0; j < js.size(); ++j)
747  jset.insert(js[j]);
748 
749  // fixed joints
750  const std::vector<const JointModel*>& fs = sg->getFixedJointModels();
751  for (std::size_t j = 0; j < fs.size(); ++j)
752  jset.insert(fs[j]);
753 
754  // mimic joints
755  const std::vector<const JointModel*>& ms = sg->getMimicJointModels();
756  for (std::size_t j = 0; j < ms.size(); ++j)
757  jset.insert(ms[j]);
758  }
759  }
760 
761  if (jset.empty())
762  {
763  ROS_WARN_NAMED(LOGNAME, "Group '%s' must have at least one valid joint", gc.name_.c_str());
764  return false;
765  }
766 
767  std::vector<const JointModel*> joints;
768  for (std::set<const JointModel*>::iterator it = jset.begin(); it != jset.end(); ++it)
769  joints.push_back(*it);
770 
771  JointModelGroup* jmg = new JointModelGroup(gc.name_, gc, joints, this);
772  joint_model_group_map_[gc.name_] = jmg;
773 
774  return true;
775 }
776 
777 JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, const srdf::Model& srdf_model)
778 {
779  // construct the joint
780  JointModel* joint = urdf_link->parent_joint ?
781  constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
782  constructJointModel(nullptr, urdf_link, srdf_model);
783  if (joint == nullptr)
784  return nullptr;
785 
786  // bookkeeping for the joint
787  joint_model_map_[joint->getName()] = joint;
788  joint->setJointIndex(joint_model_vector_.size());
789  joint_model_vector_.push_back(joint);
790  joint_model_vector_const_.push_back(joint);
791  joint_model_names_vector_.push_back(joint->getName());
792  joint->setParentLinkModel(parent);
793 
794  // construct the link
795  LinkModel* link = constructLinkModel(urdf_link);
796  joint->setChildLinkModel(link);
797  link->setParentLinkModel(parent);
798 
799  // bookkeeping for the link
800  link_model_map_[joint->getChildLinkModel()->getName()] = link;
801  link->setLinkIndex(link_model_vector_.size());
802  link_model_vector_.push_back(link);
803  link_model_vector_const_.push_back(link);
804  link_model_names_vector_.push_back(link->getName());
805  if (!link->getShapes().empty())
806  {
810  link_geometry_count_ += link->getShapes().size();
811  }
812  link->setParentJointModel(joint);
813 
814  // recursively build child links (and joints)
815  for (std::size_t i = 0; i < urdf_link->child_links.size(); ++i)
816  {
817  JointModel* jm = buildRecursive(link, urdf_link->child_links[i].get(), srdf_model);
818  if (jm)
819  link->addChildJointModel(jm);
820  }
821  return joint;
822 }
823 
824 namespace
825 {
826 // construct bounds for 1DOF joint
827 static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
828 {
829  VariableBounds b;
830  if (urdf_joint->safety)
831  {
832  b.position_bounded_ = true;
833  b.min_position_ = urdf_joint->safety->soft_lower_limit;
834  b.max_position_ = urdf_joint->safety->soft_upper_limit;
835  if (urdf_joint->limits)
836  {
837  if (urdf_joint->limits->lower > b.min_position_)
838  b.min_position_ = urdf_joint->limits->lower;
839  if (urdf_joint->limits->upper < b.max_position_)
840  b.max_position_ = urdf_joint->limits->upper;
841  }
842  }
843  else
844  {
845  if (urdf_joint->limits)
846  {
847  b.position_bounded_ = true;
848  b.min_position_ = urdf_joint->limits->lower;
849  b.max_position_ = urdf_joint->limits->upper;
850  }
851  }
852  if (urdf_joint->limits)
853  {
854  b.max_velocity_ = fabs(urdf_joint->limits->velocity);
856  b.velocity_bounded_ = b.max_velocity_ > std::numeric_limits<double>::epsilon();
857  }
858  return b;
859 }
860 } // namespace
861 
862 JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link,
863  const srdf::Model& srdf_model)
864 {
865  JointModel* result = nullptr;
866 
867  // if urdf_joint exists, must be the root link transform
868  if (urdf_joint)
869  {
870  switch (urdf_joint->type)
871  {
872  case urdf::Joint::REVOLUTE:
873  {
874  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
875  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
876  j->setContinuous(false);
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::CONTINUOUS:
882  {
883  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
884  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
885  j->setContinuous(true);
886  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
887  result = j;
888  }
889  break;
890  case urdf::Joint::PRISMATIC:
891  {
892  PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name);
893  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
894  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
895  result = j;
896  }
897  break;
898  case urdf::Joint::FLOATING:
899  result = new FloatingJointModel(urdf_joint->name);
900  break;
901  case urdf::Joint::PLANAR:
902  result = new PlanarJointModel(urdf_joint->name);
903  break;
904  case urdf::Joint::FIXED:
905  result = new FixedJointModel(urdf_joint->name);
906  break;
907  default:
908  ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type);
909  break;
910  }
911  }
912  else // if urdf_joint passed in as null, then we're at root of URDF model
913  {
914  const std::vector<srdf::Model::VirtualJoint>& virtual_joints = srdf_model.getVirtualJoints();
915  for (std::size_t i = 0; i < virtual_joints.size(); ++i)
916  {
917  if (virtual_joints[i].child_link_ != child_link->name)
918  {
919  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its child frame '%s' "
920  "does not match the URDF frame '%s'",
921  virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(),
922  child_link->name.c_str());
923  }
924  else if (virtual_joints[i].parent_frame_.empty())
925  {
926  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its parent frame is empty",
927  virtual_joints[i].name_.c_str());
928  }
929  else
930  {
931  if (virtual_joints[i].type_ == "fixed")
932  result = new FixedJointModel(virtual_joints[i].name_);
933  else if (virtual_joints[i].type_ == "planar")
934  result = new PlanarJointModel(virtual_joints[i].name_);
935  else if (virtual_joints[i].type_ == "floating")
936  result = new FloatingJointModel(virtual_joints[i].name_);
937  if (result)
938  {
939  // for fixed frames we still use the robot root link
940  if (virtual_joints[i].type_ != "fixed")
941  {
942  model_frame_ = virtual_joints[i].parent_frame_;
943  }
944  break;
945  }
946  }
947  }
948  if (!result)
949  {
950  ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint");
951  result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
952  }
953  }
954 
955  if (result)
956  {
957  result->setDistanceFactor(result->getStateSpaceDimension());
958  const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
959  for (std::size_t i = 0; i < pjoints.size(); ++i)
960  {
961  if (result->getName() == pjoints[i].name_)
962  {
963  result->setPassive(true);
964  break;
965  }
966  }
967  }
968 
969  return result;
970 }
971 
972 namespace
973 {
974 static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose)
975 {
976  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
977  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
978  return af;
979 }
980 }
981 
982 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
983 {
984  LinkModel* result = new LinkModel(urdf_link->name);
985 
986  const std::vector<urdf::CollisionSharedPtr>& col_array =
987  urdf_link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, urdf_link->collision) :
988  urdf_link->collision_array;
989 
990  std::vector<shapes::ShapeConstPtr> shapes;
992 
993  for (std::size_t i = 0; i < col_array.size(); ++i)
994  if (col_array[i] && col_array[i]->geometry)
995  {
996  shapes::ShapeConstPtr s = constructShape(col_array[i]->geometry.get());
997  if (s)
998  {
999  shapes.push_back(s);
1000  poses.push_back(urdfPose2Isometry3d(col_array[i]->origin));
1001  }
1002  }
1003  if (shapes.empty())
1004  {
1005  const std::vector<urdf::VisualSharedPtr>& vis_array = urdf_link->visual_array.empty() ?
1006  std::vector<urdf::VisualSharedPtr>(1, urdf_link->visual) :
1007  urdf_link->visual_array;
1008  for (std::size_t i = 0; i < vis_array.size(); ++i)
1009  if (vis_array[i] && vis_array[i]->geometry)
1010  {
1011  shapes::ShapeConstPtr s = constructShape(vis_array[i]->geometry.get());
1012  if (s)
1013  {
1014  shapes.push_back(s);
1015  poses.push_back(urdfPose2Isometry3d(vis_array[i]->origin));
1016  }
1017  }
1018  }
1019 
1020  result->setGeometry(shapes, poses);
1021 
1022  // figure out visual mesh (try visual urdf tag first, collision tag otherwise
1023  if (urdf_link->visual && urdf_link->visual->geometry)
1024  {
1025  if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
1026  {
1027  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
1028  if (!mesh->filename.empty())
1029  result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin),
1030  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1031  }
1032  }
1033  else if (urdf_link->collision && urdf_link->collision->geometry)
1034  {
1035  if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
1036  {
1037  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
1038  if (!mesh->filename.empty())
1039  result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin),
1040  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1041  }
1042  }
1043 
1044  if (urdf_link->parent_joint)
1045  result->setJointOriginTransform(urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
1046 
1047  return result;
1048 }
1049 
1051 {
1052  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
1053 
1054  shapes::Shape* result = nullptr;
1055  switch (geom->type)
1056  {
1057  case urdf::Geometry::SPHERE:
1058  result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
1059  break;
1060  case urdf::Geometry::BOX:
1061  {
1062  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
1063  result = new shapes::Box(dim.x, dim.y, dim.z);
1064  }
1065  break;
1066  case urdf::Geometry::CYLINDER:
1067  result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
1068  static_cast<const urdf::Cylinder*>(geom)->length);
1069  break;
1070  case urdf::Geometry::MESH:
1071  {
1072  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
1073  if (!mesh->filename.empty())
1074  {
1075  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
1076  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
1077  result = m;
1078  }
1079  }
1080  break;
1081  default:
1082  ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type);
1083  break;
1084  }
1085 
1086  return shapes::ShapePtr(result);
1087 }
1088 
1089 bool RobotModel::hasJointModel(const std::string& name) const
1090 {
1091  return joint_model_map_.find(name) != joint_model_map_.end();
1092 }
1093 
1094 bool RobotModel::hasLinkModel(const std::string& name) const
1095 {
1096  return link_model_map_.find(name) != link_model_map_.end();
1097 }
1098 
1099 const JointModel* RobotModel::getJointModel(const std::string& name) const
1100 {
1101  JointModelMap::const_iterator it = joint_model_map_.find(name);
1102  if (it != joint_model_map_.end())
1103  return it->second;
1104  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1105  return nullptr;
1106 }
1107 
1108 const JointModel* RobotModel::getJointModel(int index) const
1109 {
1110  if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
1111  {
1112  ROS_ERROR_NAMED(LOGNAME, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
1113  return nullptr;
1114  }
1115  assert(joint_model_vector_[index]->getJointIndex() == index);
1116  return joint_model_vector_[index];
1117 }
1118 
1119 JointModel* RobotModel::getJointModel(const std::string& name)
1120 {
1121  JointModelMap::const_iterator it = joint_model_map_.find(name);
1122  if (it != joint_model_map_.end())
1123  return it->second;
1124  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1125  return nullptr;
1126 }
1127 
1128 const LinkModel* RobotModel::getLinkModel(const std::string& name) const
1129 {
1130  return const_cast<RobotModel*>(this)->getLinkModel(name);
1131 }
1132 
1133 const LinkModel* RobotModel::getLinkModel(int index) const
1134 {
1135  if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
1136  {
1137  ROS_ERROR_NAMED(LOGNAME, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
1138  return nullptr;
1139  }
1140  assert(link_model_vector_[index]->getLinkIndex() == index);
1141  return link_model_vector_[index];
1142 }
1143 
1144 LinkModel* RobotModel::getLinkModel(const std::string& name)
1145 {
1146  LinkModelMap::const_iterator it = link_model_map_.find(name);
1147  if (it != link_model_map_.end())
1148  return it->second;
1149  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1150  return nullptr;
1151 }
1152 
1154 {
1155  if (!link)
1156  return link;
1157  const robot_model::LinkModel* parent_link = link->getParentLinkModel();
1158  const robot_model::JointModel* joint = link->getParentJointModel();
1159 
1160  while (parent_link && joint->getType() == robot_model::JointModel::FIXED)
1161  {
1162  link = parent_link;
1163  joint = link->getParentJointModel();
1164  parent_link = joint->getParentLinkModel();
1165  }
1166  return link;
1167 }
1168 
1169 void RobotModel::updateMimicJoints(double* values) const
1170 {
1171  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
1172  {
1173  int src = mimic_joints_[i]->getMimic()->getFirstVariableIndex();
1174  int dest = mimic_joints_[i]->getFirstVariableIndex();
1175  values[dest] = values[src] * mimic_joints_[i]->getMimicFactor() + mimic_joints_[i]->getMimicOffset();
1176  }
1177 }
1178 
1180 {
1181  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1183  updateMimicJoints(values);
1184 }
1185 
1187  std::map<std::string, double>& values) const
1188 {
1189  std::vector<double> tmp(variable_count_);
1190  getVariableRandomPositions(rng, &tmp[0]);
1191  values.clear();
1192  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1193  values[variable_names_[i]] = tmp[i];
1194 }
1195 
1197 {
1198  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1200  updateMimicJoints(values);
1201 }
1202 
1203 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1204 {
1205  std::vector<double> tmp(variable_count_);
1206  getVariableDefaultPositions(&tmp[0]);
1207  values.clear();
1208  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1209  values[variable_names_[i]] = tmp[i];
1210 }
1211 
1212 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1213  std::vector<std::string>& missing_variables) const
1214 {
1215  missing_variables.clear();
1216  std::set<std::string> keys(variables.begin(), variables.end());
1217  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1218  if (keys.find(variable_names_[i]) == keys.end())
1219  if (getJointOfVariable(variable_names_[i])->getMimic() == nullptr)
1220  missing_variables.push_back(variable_names_[i]);
1221 }
1222 
1223 int RobotModel::getVariableIndex(const std::string& variable) const
1224 {
1225  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1226  if (it == joint_variables_index_map_.end())
1227  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1228  return it->second;
1229 }
1230 
1231 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1232 {
1233  double max_distance = 0.0;
1234  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1235  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1236  active_joint_model_vector_[j]->getDistanceFactor();
1237  return max_distance;
1238 }
1239 
1240 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1241  double margin) const
1242 {
1243  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1244  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1246  *active_joint_bounds[i], margin))
1247  return false;
1248  return true;
1249 }
1250 
1251 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1252 {
1253  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1254  bool change = false;
1255  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1257  *active_joint_bounds[i]))
1258  change = true;
1259  if (change)
1260  updateMimicJoints(state);
1261  return change;
1262 }
1263 
1264 double RobotModel::distance(const double* state1, const double* state2) const
1265 {
1266  double d = 0.0;
1267  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1268  d += active_joint_model_vector_[i]->getDistanceFactor() *
1270  state2 + active_joint_model_start_index_[i]);
1271  return d;
1272 }
1273 
1274 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1275 {
1276  // we interpolate values only for active joint models (non-mimic)
1277  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1280  state + active_joint_model_start_index_[i]);
1281  // now we update mimic as needed
1282  updateMimicJoints(state);
1283 }
1284 
1285 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1286 {
1287  // we first set all the "simple" allocators -- where a group has one IK solver
1289  {
1290  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1291  if (jt != allocators.end())
1292  {
1293  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1294  result.first = jt->second;
1295  jmg->setSolverAllocators(result);
1296  }
1297  }
1298 
1299  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1300  // setSolverAllocators()
1301  for (JointModelGroup* jmg : joint_model_groups_)
1302  {
1303  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1304  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1305  if (jt == allocators.end())
1306  {
1307  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1308  std::set<const JointModel*> joints;
1309  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1310 
1311  std::vector<const JointModelGroup*> subs;
1312 
1313  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1314  for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin(); kt != allocators.end();
1315  ++kt)
1316  {
1317  const JointModelGroup* sub = getJointModelGroup(kt->first);
1318  if (!sub) // this should actually not happen, all groups should be well defined
1319  {
1320  subs.clear();
1321  break;
1322  }
1323  std::set<const JointModel*> sub_joints;
1324  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1325 
1326  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1327  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1328  std::set<const JointModel*> resultj;
1329  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1330  std::inserter(resultj, resultj.end()));
1331  // TODO: instead of maintaining disjoint joint sets here,
1332  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1333  // There, a disjoint bijection from joints to solvers is computed anyway.
1334  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1335  // But, if the overlap only involves fixed joints, we could consider all sub groups
1336  subs.push_back(sub);
1337  joints.swap(resultj);
1338  }
1339  }
1340 
1341  // if we found subgroups, pass that information to the planning group
1342  if (!subs.empty())
1343  {
1344  std::stringstream ss;
1345  for (std::size_t i = 0; i < subs.size(); ++i)
1346  {
1347  ss << subs[i]->getName() << " ";
1348  result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
1349  }
1350  ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1351  ss.str().c_str());
1352  }
1353  jmg->setSolverAllocators(result);
1354  }
1355  }
1356 }
1357 
1358 void RobotModel::printModelInfo(std::ostream& out) const
1359 {
1360  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
1361  << std::endl;
1362 
1363  std::ios_base::fmtflags old_flags = out.flags();
1364  out.setf(std::ios::fixed, std::ios::floatfield);
1365  std::streamsize old_prec = out.precision();
1366  out.precision(5);
1367  out << "Joints: " << std::endl;
1368  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
1369  {
1370  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
1371  << std::endl;
1372  out << " * Joint Index: " << joint_model_vector_[i]->getJointIndex() << std::endl;
1373  const std::vector<std::string>& vn = joint_model_vector_[i]->getVariableNames();
1374  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
1375  << std::endl;
1376  int idx = joint_model_vector_[i]->getFirstVariableIndex();
1377  for (std::vector<std::string>::const_iterator it = vn.begin(); it != vn.end(); ++it)
1378  {
1379  out << " * '" << *it << "', index " << idx++ << " in full state";
1380  if (joint_model_vector_[i]->getMimic())
1381  out << ", mimic '" << joint_model_vector_[i]->getMimic()->getName() << "'";
1382  if (joint_model_vector_[i]->isPassive())
1383  out << ", passive";
1384  out << std::endl;
1385  out << " " << joint_model_vector_[i]->getVariableBounds(*it) << std::endl;
1386  }
1387  }
1388  out << std::endl;
1389  out.precision(old_prec);
1390  out.flags(old_flags);
1391  out << "Links: " << std::endl;
1392  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
1393  {
1394  out << " '" << link_model_vector_[i]->getName() << "' with " << link_model_vector_[i]->getShapes().size()
1395  << " geoms" << std::endl;
1396  if (link_model_vector_[i]->parentJointIsFixed())
1397  out << " * "
1398  << "parent joint is fixed" << std::endl;
1399  if (link_model_vector_[i]->jointOriginTransformIsIdentity())
1400  out << " * "
1401  << "joint origin transform is identity" << std::endl;
1402  }
1403 
1404  out << "Available groups: " << std::endl;
1405  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
1406  joint_model_groups_[i]->printGroupInfo(out);
1407 }
1408 
1409 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
1410  LinkTransformMap& associated_transforms)
1411 {
1412  associated_transforms[link] = transform * link->getJointOriginTransform();
1413  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1414  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1415  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1416  transform * link->getJointOriginTransform(), associated_transforms);
1417 }
1418 
1419 } // end of namespace core
1420 } // 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
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
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
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
#define ROS_WARN_STREAM_NAMED(name, args)
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 Thu Apr 2 2020 03:50:31