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 <cmath>
45 #include <memory>
46 #include "order_robot_model_items.inc"
47 
48 namespace moveit
49 {
50 namespace core
51 {
52 const std::string LOGNAME = "robot_model";
53 
54 RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model)
55 {
56  root_joint_ = nullptr;
57  urdf_ = urdf_model;
58  srdf_ = srdf_model;
59  buildModel(*urdf_model, *srdf_model);
60 }
61 
63 {
64  for (JointModelGroupMap::iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
65  delete it->second;
66  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
67  delete joint_model_vector_[i];
68  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
69  delete link_model_vector_[i];
70 }
71 
73 {
74  return root_joint_;
75 }
76 
78 {
79  return root_link_;
80 }
81 
82 void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model)
83 {
85  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildModel");
86 
87  root_joint_ = nullptr;
88  root_link_ = nullptr;
90  variable_count_ = 0;
91  model_name_ = urdf_model.getName();
92  ROS_INFO_NAMED(LOGNAME, "Loading robot model '%s'...", model_name_.c_str());
93 
94  if (urdf_model.getRoot())
95  {
96  const urdf::Link* root_link_ptr = urdf_model.getRoot().get();
97  model_frame_ = root_link_ptr->name;
98 
99  ROS_DEBUG_NAMED(LOGNAME, "... building kinematic chain");
100  root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model);
101  if (root_joint_)
103  ROS_DEBUG_NAMED(LOGNAME, "... building mimic joints");
104  buildMimic(urdf_model);
105 
106  ROS_DEBUG_NAMED(LOGNAME, "... computing joint indexing");
107  buildJointInfo();
108 
110  ROS_WARN_NAMED(LOGNAME, "No geometry is associated to any robot links");
111 
112  // build groups
113 
114  ROS_DEBUG_NAMED(LOGNAME, "... constructing joint groups");
115  buildGroups(srdf_model);
116 
117  ROS_DEBUG_NAMED(LOGNAME, "... constructing joint group states");
118  buildGroupStates(srdf_model);
119 
120  // For debugging entire model
121  // printModelInfo(std::cout);
122  }
123  else
124  ROS_WARN_NAMED(LOGNAME, "No root link found");
125 }
126 
127 namespace
128 {
129 typedef std::map<const JointModel*, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
130  std::set<const JointModel*, OrderJointsByIndex> > >
131  DescMap;
132 
133 void computeDescendantsHelper(const JointModel* joint, std::vector<const JointModel*>& parents,
134  std::set<const JointModel*>& seen, DescMap& descendants)
135 {
136  if (!joint)
137  return;
138  if (seen.find(joint) != seen.end())
139  return;
140  seen.insert(joint);
141 
142  for (std::size_t i = 0; i < parents.size(); ++i)
143  descendants[parents[i]].second.insert(joint);
144 
145  const LinkModel* lm = joint->getChildLinkModel();
146  if (!lm)
147  return;
148 
149  for (std::size_t i = 0; i < parents.size(); ++i)
150  descendants[parents[i]].first.insert(lm);
151  descendants[joint].first.insert(lm);
152 
153  parents.push_back(joint);
154  const std::vector<const JointModel*>& ch = lm->getChildJointModels();
155  for (std::size_t i = 0; i < ch.size(); ++i)
156  computeDescendantsHelper(ch[i], parents, seen, descendants);
157  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
158  for (std::size_t i = 0; i < mim.size(); ++i)
159  computeDescendantsHelper(mim[i], parents, seen, descendants);
160  parents.pop_back();
161 }
162 
163 void computeCommonRootsHelper(const JointModel* joint, std::vector<int>& common_roots, int size)
164 {
165  if (!joint)
166  return;
167  const LinkModel* lm = joint->getChildLinkModel();
168  if (!lm)
169  return;
170 
171  const std::vector<const JointModel*>& ch = lm->getChildJointModels();
172  for (std::size_t i = 0; i < ch.size(); ++i)
173  {
174  const std::vector<const JointModel*>& a = ch[i]->getDescendantJointModels();
175  for (std::size_t j = i + 1; j < ch.size(); ++j)
176  {
177  const std::vector<const JointModel*>& b = ch[j]->getDescendantJointModels();
178  for (std::size_t m = 0; m < b.size(); ++m)
179  common_roots[ch[i]->getJointIndex() * size + b[m]->getJointIndex()] =
180  common_roots[ch[i]->getJointIndex() + b[m]->getJointIndex() * size] = joint->getJointIndex();
181  for (std::size_t k = 0; k < a.size(); ++k)
182  {
183  common_roots[a[k]->getJointIndex() * size + ch[j]->getJointIndex()] =
184  common_roots[a[k]->getJointIndex() + ch[j]->getJointIndex() * size] = joint->getJointIndex();
185  for (std::size_t m = 0; m < b.size(); ++m)
186  common_roots[a[k]->getJointIndex() * size + b[m]->getJointIndex()] =
187  common_roots[a[k]->getJointIndex() + b[m]->getJointIndex() * size] = joint->getJointIndex();
188  }
189  }
190  computeCommonRootsHelper(ch[i], common_roots, size);
191  }
192 }
193 } // namespace
194 
196 {
197  // compute common roots for all pairs of joints;
198  // there are 3 cases of pairs (X, Y):
199  // X != Y && X and Y are not descendants of one another
200  // X == Y
201  // X != Y && X and Y are descendants of one another
202 
203  // by default, the common root is always the global root;
205 
206  // look at all descendants recursively; for two sibling nodes A, B, both children of X, all the pairs of respective
207  // descendants of A and B
208  // have X as the common root.
209  computeCommonRootsHelper(root_joint_, common_joint_roots_, joint_model_vector_.size());
210 
211  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
212  {
213  // the common root of a joint and itself is the same joint:
214  common_joint_roots_[joint_model_vector_[i]->getJointIndex() * (1 + joint_model_vector_.size())] =
215  joint_model_vector_[i]->getJointIndex();
216 
217  // a node N and one of its descendants have as common root the node N itself:
218  const std::vector<const JointModel*>& d = joint_model_vector_[i]->getDescendantJointModels();
219  for (std::size_t j = 0; j < d.size(); ++j)
220  common_joint_roots_[d[j]->getJointIndex() * joint_model_vector_.size() + joint_model_vector_[i]->getJointIndex()] =
221  common_joint_roots_[d[j]->getJointIndex() +
222  joint_model_vector_[i]->getJointIndex() * joint_model_vector_.size()] =
223  joint_model_vector_[i]->getJointIndex();
224  }
225 }
226 
228 {
229  // compute the list of descendants for all joints
230  std::vector<const JointModel*> parents;
231  std::set<const JointModel*> seen;
232 
233  DescMap descendants;
234  computeDescendantsHelper(root_joint_, parents, seen, descendants);
235  for (DescMap::iterator it = descendants.begin(); it != descendants.end(); ++it)
236  {
237  JointModel* jm = const_cast<JointModel*>(it->first);
238  for (std::set<const JointModel*>::const_iterator jt = it->second.second.begin(); jt != it->second.second.end(); ++jt)
239  jm->addDescendantJointModel(*jt);
240  for (std::set<const LinkModel*>::const_iterator jt = it->second.first.begin(); jt != it->second.first.end(); ++jt)
241  jm->addDescendantLinkModel(*jt);
242  }
243 }
244 
246 {
248  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildJointInfo");
249 
250  // construct additional maps for easy access by name
251  variable_count_ = 0;
253  variable_names_.reserve(joint_model_vector_.size());
255 
256  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
257  {
258  joint_model_vector_[i]->setJointIndex(i);
259  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
260 
261  // compute index map
262  if (!name_order.empty())
263  {
264  for (std::size_t j = 0; j < name_order.size(); ++j)
265  {
266  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
267  variable_names_.push_back(name_order[j]);
269  }
270  if (joint_model_vector_[i]->getMimic() == nullptr)
271  {
276  }
277 
278  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
279  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
281 
282  joint_model_vector_[i]->setFirstVariableIndex(variable_count_);
284 
285  // compute variable count
286  std::size_t vc = joint_model_vector_[i]->getVariableCount();
287  variable_count_ += vc;
288  if (vc == 1)
289  single_dof_joints_.push_back(joint_model_vector_[i]);
290  else
291  multi_dof_joints_.push_back(joint_model_vector_[i]);
292  }
293  }
294 
295  std::vector<bool> link_considered(link_model_vector_.size(), false);
296  for (const LinkModel* link : link_model_vector_)
297  {
298  if (link_considered[link->getLinkIndex()])
299  continue;
300 
301  LinkTransformMap associated_transforms;
302  computeFixedTransforms(link, link->getJointOriginTransform().inverse(), associated_transforms);
303  for (auto& tf_base : associated_transforms)
304  {
305  link_considered[tf_base.first->getLinkIndex()] = true;
306  for (auto& tf_target : associated_transforms)
307  {
308  if (&tf_base != &tf_target)
309  const_cast<LinkModel*>(tf_base.first) // regain write access to base LinkModel*
310  ->addAssociatedFixedTransform(tf_target.first, tf_base.second.inverse() * tf_target.second);
311  }
312  }
313  }
314 
316  computeCommonRoots(); // must be called _after_ list of descendants was computed
317 }
318 
320 {
321  // copy the default states to the groups
322  const std::vector<srdf::Model::GroupState>& ds = srdf_model.getGroupStates();
323  for (std::size_t i = 0; i < ds.size(); ++i)
324  {
325  if (hasJointModelGroup(ds[i].group_))
326  {
327  JointModelGroup* jmg = getJointModelGroup(ds[i].group_);
328  std::vector<const JointModel*> remaining_joints = jmg->getActiveJointModels();
329  std::map<std::string, double> state;
330  for (std::map<std::string, std::vector<double> >::const_iterator jt = ds[i].joint_values_.begin();
331  jt != ds[i].joint_values_.end(); ++jt)
332  {
333  if (jmg->hasJointModel(jt->first))
334  {
335  const JointModel* jm = jmg->getJointModel(jt->first);
336  const std::vector<std::string>& vn = jm->getVariableNames();
337  // Remove current joint name from remaining list.
338  auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm);
339  if (it_found != remaining_joints.end())
340  remaining_joints.erase(it_found);
341  if (vn.size() == jt->second.size())
342  for (std::size_t j = 0; j < vn.size(); ++j)
343  state[vn[j]] = jt->second[j];
344  else
345  ROS_ERROR_NAMED(LOGNAME,
346  "The model for joint '%s' requires %d variable values, "
347  "but only %d variable values were supplied in default state '%s' for group '%s'",
348  jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), ds[i].name_.c_str(),
349  jmg->getName().c_str());
350  }
351  else
352  ROS_ERROR_NAMED(LOGNAME,
353  "Group state '%s' specifies value for joint '%s', "
354  "but that joint is not part of group '%s'",
355  ds[i].name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
356  }
357  if (!remaining_joints.empty())
358  {
359  std::stringstream missing;
360  missing << (*remaining_joints.begin())->getName();
361  for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); j++)
362  {
363  missing << ", " << (*j)->getName();
364  }
365  ROS_WARN_STREAM_NAMED(LOGNAME, "Group state '" << ds[i].name_ << "' doesn't specify all group joints in group '"
366  << ds[i].group_ << "'. " << missing.str() << " "
367  << (remaining_joints.size() > 1 ? "are" : "is") << " missing.");
368  }
369  if (!state.empty())
370  jmg->addDefaultState(ds[i].name_, state);
371  }
372  else
373  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specified for group '%s', but that group does not exist",
374  ds[i].name_.c_str(), ds[i].group_.c_str());
375  }
376 }
377 
378 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
379 {
380  // compute mimic joints
381  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
382  {
383  const urdf::Joint* jm = urdf_model.getJoint(joint_model_vector_[i]->getName()).get();
384  if (jm)
385  if (jm->mimic)
386  {
387  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
388  if (jit != joint_model_map_.end())
389  {
390  if (joint_model_vector_[i]->getVariableCount() == jit->second->getVariableCount())
391  joint_model_vector_[i]->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
392  else
393  ROS_ERROR_NAMED(LOGNAME, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
394  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
395  }
396  else
397  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' cannot mimic unknown joint '%s'",
398  joint_model_vector_[i]->getName().c_str(), jm->mimic->joint_name.c_str());
399  }
400  }
401 
402  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
403  bool change = true;
404  while (change)
405  {
406  change = false;
407  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
408  if (joint_model_vector_[i]->getMimic())
409  {
410  if (joint_model_vector_[i]->getMimic()->getMimic())
411  {
412  joint_model_vector_[i]->setMimic(
413  joint_model_vector_[i]->getMimic()->getMimic(),
414  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicFactor(),
415  joint_model_vector_[i]->getMimicOffset() +
416  joint_model_vector_[i]->getMimicFactor() * joint_model_vector_[i]->getMimic()->getMimicOffset());
417  change = true;
418  }
419  if (joint_model_vector_[i] == joint_model_vector_[i]->getMimic())
420  {
421  ROS_ERROR_NAMED(LOGNAME, "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
422  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
423  joint_model_vector_[i]->setMimic(nullptr, 0.0, 0.0);
424  change = false;
425  break;
426  }
427  }
428  }
429  // build mimic requests
430  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
431  if (joint_model_vector_[i]->getMimic())
432  {
433  const_cast<JointModel*>(joint_model_vector_[i]->getMimic())->addMimicRequest(joint_model_vector_[i]);
434  mimic_joints_.push_back(joint_model_vector_[i]);
435  }
436 }
437 
438 bool RobotModel::hasEndEffector(const std::string& eef) const
439 {
440  return end_effectors_map_.find(eef) != end_effectors_map_.end();
441 }
442 
443 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
444 {
445  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
446  if (it == end_effectors_map_.end())
447  {
448  it = joint_model_group_map_.find(name);
449  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
450  return it->second;
451  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
452  return nullptr;
453  }
454  return it->second;
455 }
456 
458 {
459  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
460  if (it == end_effectors_map_.end())
461  {
462  it = joint_model_group_map_.find(name);
463  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
464  return it->second;
465  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
466  return nullptr;
467  }
468  return it->second;
469 }
470 
471 bool RobotModel::hasJointModelGroup(const std::string& name) const
472 {
473  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
474 }
475 
476 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
477 {
478  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
479  if (it == joint_model_group_map_.end())
480  {
481  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
482  return nullptr;
483  }
484  return it->second;
485 }
486 
488 {
489  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
490  if (it == joint_model_group_map_.end())
491  {
492  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
493  return nullptr;
494  }
495  return it->second;
496 }
497 
498 void RobotModel::buildGroups(const srdf::Model& srdf_model)
499 {
500  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
501 
502  // the only thing tricky is dealing with subgroups
503  std::vector<bool> processed(group_configs.size(), false);
504 
505  bool added = true;
506  while (added)
507  {
508  added = false;
509 
510  // going to make passes until we can't do anything else
511  for (std::size_t i = 0; i < group_configs.size(); ++i)
512  if (!processed[i])
513  {
514  // if we haven't processed, check and see if the dependencies are met yet
515  bool all_subgroups_added = true;
516  for (std::size_t j = 0; j < group_configs[i].subgroups_.size(); ++j)
517  if (joint_model_group_map_.find(group_configs[i].subgroups_[j]) == joint_model_group_map_.end())
518  {
519  all_subgroups_added = false;
520  break;
521  }
522  if (all_subgroups_added)
523  {
524  added = true;
525  processed[i] = true;
526  if (!addJointModelGroup(group_configs[i]))
527  ROS_WARN_NAMED(LOGNAME, "Failed to add group '%s'", group_configs[i].name_.c_str());
528  }
529  }
530  }
531 
532  for (std::size_t i = 0; i < processed.size(); ++i)
533  if (!processed[i])
534  ROS_WARN_NAMED(LOGNAME, "Could not process group '%s' due to unmet subgroup dependencies",
535  group_configs[i].name_.c_str());
536 
537  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
538  joint_model_groups_.push_back(it->second);
539  std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
540  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
541  {
544  }
545 
546  buildGroupsInfoSubgroups(srdf_model);
547  buildGroupsInfoEndEffectors(srdf_model);
548 }
549 
551 {
552  // compute subgroups
553  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
554  {
555  JointModelGroup* jmg = it->second;
556  std::vector<std::string> subgroup_names;
557  std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
558  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
559  ++jt)
560  if (jt->first != it->first)
561  {
562  bool ok = true;
563  JointModelGroup* sub_jmg = jt->second;
564  const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
565  for (std::size_t k = 0; k < sub_joints.size(); ++k)
566  if (joints.find(sub_joints[k]) == joints.end())
567  {
568  ok = false;
569  break;
570  }
571  if (ok)
572  subgroup_names.push_back(sub_jmg->getName());
573  }
574  if (!subgroup_names.empty())
575  jmg->setSubgroupNames(subgroup_names);
576  }
577 }
578 
580 {
581  // set the end-effector flags
582  const std::vector<srdf::Model::EndEffector>& eefs = srdf_model.getEndEffectors();
583  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
584  {
585  // check if this group is a known end effector
586  for (std::size_t k = 0; k < eefs.size(); ++k)
587  if (eefs[k].component_group_ == it->first)
588  {
589  // if it is, mark it as such
590  it->second->setEndEffectorName(eefs[k].name_);
591  end_effectors_map_[eefs[k].name_] = it->second;
592  end_effectors_.push_back(it->second);
593 
594  // check to see if there are groups that contain the parent link of this end effector.
595  // record this information if found;
596  std::vector<JointModelGroup*> possible_parent_groups;
597  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
598  ++jt)
599  if (jt->first != it->first)
600  {
601  if (jt->second->hasLinkModel(eefs[k].parent_link_))
602  {
603  jt->second->attachEndEffector(eefs[k].name_);
604  possible_parent_groups.push_back(jt->second);
605  }
606  }
607 
608  JointModelGroup* eef_parent_group = nullptr;
609  // if a parent group is specified in SRDF, try to use it
610  if (!eefs[k].parent_group_.empty())
611  {
612  JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eefs[k].parent_group_);
613  if (jt != joint_model_group_map_.end())
614  {
615  if (jt->second->hasLinkModel(eefs[k].parent_link_))
616  {
617  if (jt->second != it->second)
618  eef_parent_group = jt->second;
619  else
620  ROS_ERROR_NAMED(LOGNAME, "Group '%s' for end-effector '%s' cannot be its own parent",
621  eefs[k].parent_group_.c_str(), eefs[k].name_.c_str());
622  }
623  else
624  ROS_ERROR_NAMED(LOGNAME,
625  "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,
920  "Skipping virtual joint '%s' because its child frame '%s' "
921  "does not match the URDF frame '%s'",
922  virtual_joints[i].name_.c_str(), virtual_joints[i].child_link_.c_str(),
923  child_link->name.c_str());
924  }
925  else if (virtual_joints[i].parent_frame_.empty())
926  {
927  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its parent frame is empty",
928  virtual_joints[i].name_.c_str());
929  }
930  else
931  {
932  if (virtual_joints[i].type_ == "fixed")
933  result = new FixedJointModel(virtual_joints[i].name_);
934  else if (virtual_joints[i].type_ == "planar")
935  result = new PlanarJointModel(virtual_joints[i].name_);
936  else if (virtual_joints[i].type_ == "floating")
937  result = new FloatingJointModel(virtual_joints[i].name_);
938  if (result)
939  {
940  // for fixed frames we still use the robot root link
941  if (virtual_joints[i].type_ != "fixed")
942  {
943  model_frame_ = virtual_joints[i].parent_frame_;
944  }
945  break;
946  }
947  }
948  }
949  if (!result)
950  {
951  ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint");
952  result = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
953  }
954  }
955 
956  if (result)
957  {
958  result->setDistanceFactor(result->getStateSpaceDimension());
959  const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
960  for (std::size_t i = 0; i < pjoints.size(); ++i)
961  {
962  if (result->getName() == pjoints[i].name_)
963  {
964  result->setPassive(true);
965  break;
966  }
967  }
968  }
969 
970  return result;
971 }
972 
973 namespace
974 {
975 static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose)
976 {
977  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
978  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
979  return af;
980 }
981 } // namespace
982 
983 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
984 {
985  LinkModel* result = new LinkModel(urdf_link->name);
986 
987  const std::vector<urdf::CollisionSharedPtr>& col_array =
988  urdf_link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, urdf_link->collision) :
989  urdf_link->collision_array;
990 
991  std::vector<shapes::ShapeConstPtr> shapes;
993 
994  for (std::size_t i = 0; i < col_array.size(); ++i)
995  if (col_array[i] && col_array[i]->geometry)
996  {
997  shapes::ShapeConstPtr s = constructShape(col_array[i]->geometry.get());
998  if (s)
999  {
1000  shapes.push_back(s);
1001  poses.push_back(urdfPose2Isometry3d(col_array[i]->origin));
1002  }
1003  }
1004  if (shapes.empty())
1005  {
1006  const std::vector<urdf::VisualSharedPtr>& vis_array = urdf_link->visual_array.empty() ?
1007  std::vector<urdf::VisualSharedPtr>(1, urdf_link->visual) :
1008  urdf_link->visual_array;
1009  for (std::size_t i = 0; i < vis_array.size(); ++i)
1010  if (vis_array[i] && vis_array[i]->geometry)
1011  {
1012  shapes::ShapeConstPtr s = constructShape(vis_array[i]->geometry.get());
1013  if (s)
1014  {
1015  shapes.push_back(s);
1016  poses.push_back(urdfPose2Isometry3d(vis_array[i]->origin));
1017  }
1018  }
1019  }
1020 
1021  result->setGeometry(shapes, poses);
1022 
1023  // figure out visual mesh (try visual urdf tag first, collision tag otherwise
1024  if (urdf_link->visual && urdf_link->visual->geometry)
1025  {
1026  if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
1027  {
1028  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
1029  if (!mesh->filename.empty())
1030  result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin),
1031  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1032  }
1033  }
1034  else if (urdf_link->collision && urdf_link->collision->geometry)
1035  {
1036  if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
1037  {
1038  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
1039  if (!mesh->filename.empty())
1040  result->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin),
1041  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1042  }
1043  }
1044 
1045  if (urdf_link->parent_joint)
1046  result->setJointOriginTransform(urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
1047 
1048  return result;
1049 }
1050 
1052 {
1053  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
1054 
1055  shapes::Shape* result = nullptr;
1056  switch (geom->type)
1057  {
1058  case urdf::Geometry::SPHERE:
1059  result = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
1060  break;
1061  case urdf::Geometry::BOX:
1062  {
1063  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
1064  result = new shapes::Box(dim.x, dim.y, dim.z);
1065  }
1066  break;
1067  case urdf::Geometry::CYLINDER:
1068  result = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
1069  static_cast<const urdf::Cylinder*>(geom)->length);
1070  break;
1071  case urdf::Geometry::MESH:
1072  {
1073  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
1074  if (!mesh->filename.empty())
1075  {
1076  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
1077  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
1078  result = m;
1079  }
1080  }
1081  break;
1082  default:
1083  ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type);
1084  break;
1085  }
1086 
1087  return shapes::ShapePtr(result);
1088 }
1089 
1090 bool RobotModel::hasJointModel(const std::string& name) const
1091 {
1092  return joint_model_map_.find(name) != joint_model_map_.end();
1093 }
1094 
1095 bool RobotModel::hasLinkModel(const std::string& name) const
1096 {
1097  return link_model_map_.find(name) != link_model_map_.end();
1098 }
1099 
1100 const JointModel* RobotModel::getJointModel(const std::string& name) const
1101 {
1102  JointModelMap::const_iterator it = joint_model_map_.find(name);
1103  if (it != joint_model_map_.end())
1104  return it->second;
1105  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1106  return nullptr;
1107 }
1108 
1109 const JointModel* RobotModel::getJointModel(int index) const
1110 {
1111  if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
1112  {
1113  ROS_ERROR_NAMED(LOGNAME, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
1114  return nullptr;
1115  }
1116  assert(joint_model_vector_[index]->getJointIndex() == index);
1117  return joint_model_vector_[index];
1118 }
1119 
1120 JointModel* RobotModel::getJointModel(const std::string& name)
1121 {
1122  JointModelMap::const_iterator it = joint_model_map_.find(name);
1123  if (it != joint_model_map_.end())
1124  return it->second;
1125  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1126  return nullptr;
1127 }
1128 
1129 const LinkModel* RobotModel::getLinkModel(const std::string& name) const
1130 {
1131  return const_cast<RobotModel*>(this)->getLinkModel(name);
1132 }
1133 
1134 const LinkModel* RobotModel::getLinkModel(int index) const
1135 {
1136  if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
1137  {
1138  ROS_ERROR_NAMED(LOGNAME, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
1139  return nullptr;
1140  }
1141  assert(link_model_vector_[index]->getLinkIndex() == index);
1142  return link_model_vector_[index];
1143 }
1144 
1145 LinkModel* RobotModel::getLinkModel(const std::string& name)
1146 {
1147  LinkModelMap::const_iterator it = link_model_map_.find(name);
1148  if (it != link_model_map_.end())
1149  return it->second;
1150  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1151  return nullptr;
1152 }
1153 
1155 {
1156  if (!link)
1157  return link;
1158  const robot_model::LinkModel* parent_link = link->getParentLinkModel();
1159  const robot_model::JointModel* joint = link->getParentJointModel();
1160 
1161  while (parent_link && joint->getType() == robot_model::JointModel::FIXED)
1162  {
1163  link = parent_link;
1164  joint = link->getParentJointModel();
1165  parent_link = joint->getParentLinkModel();
1166  }
1167  return link;
1168 }
1169 
1170 void RobotModel::updateMimicJoints(double* values) const
1171 {
1172  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
1173  {
1174  int src = mimic_joints_[i]->getMimic()->getFirstVariableIndex();
1175  int dest = mimic_joints_[i]->getFirstVariableIndex();
1176  values[dest] = values[src] * mimic_joints_[i]->getMimicFactor() + mimic_joints_[i]->getMimicOffset();
1177  }
1178 }
1179 
1181 {
1182  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1184  updateMimicJoints(values);
1185 }
1186 
1188  std::map<std::string, double>& values) const
1189 {
1190  std::vector<double> tmp(variable_count_);
1191  getVariableRandomPositions(rng, &tmp[0]);
1192  values.clear();
1193  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1194  values[variable_names_[i]] = tmp[i];
1195 }
1196 
1198 {
1199  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1201  updateMimicJoints(values);
1202 }
1203 
1204 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1205 {
1206  std::vector<double> tmp(variable_count_);
1207  getVariableDefaultPositions(&tmp[0]);
1208  values.clear();
1209  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1210  values[variable_names_[i]] = tmp[i];
1211 }
1212 
1213 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1214  std::vector<std::string>& missing_variables) const
1215 {
1216  missing_variables.clear();
1217  std::set<std::string> keys(variables.begin(), variables.end());
1218  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1219  if (keys.find(variable_names_[i]) == keys.end())
1220  if (getJointOfVariable(variable_names_[i])->getMimic() == nullptr)
1221  missing_variables.push_back(variable_names_[i]);
1222 }
1223 
1224 int RobotModel::getVariableIndex(const std::string& variable) const
1225 {
1226  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1227  if (it == joint_variables_index_map_.end())
1228  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1229  return it->second;
1230 }
1231 
1232 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1233 {
1234  double max_distance = 0.0;
1235  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1236  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1237  active_joint_model_vector_[j]->getDistanceFactor();
1238  return max_distance;
1239 }
1240 
1241 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1242  double margin) const
1243 {
1244  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1245  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1247  *active_joint_bounds[i], margin))
1248  return false;
1249  return true;
1250 }
1251 
1252 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1253 {
1254  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1255  bool change = false;
1256  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1258  *active_joint_bounds[i]))
1259  change = true;
1260  if (change)
1261  updateMimicJoints(state);
1262  return change;
1263 }
1264 
1265 double RobotModel::distance(const double* state1, const double* state2) const
1266 {
1267  double d = 0.0;
1268  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1269  d += active_joint_model_vector_[i]->getDistanceFactor() *
1271  state2 + active_joint_model_start_index_[i]);
1272  return d;
1273 }
1274 
1275 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1276 {
1277  // we interpolate values only for active joint models (non-mimic)
1278  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1281  state + active_joint_model_start_index_[i]);
1282  // now we update mimic as needed
1283  updateMimicJoints(state);
1284 }
1285 
1286 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1287 {
1288  // we first set all the "simple" allocators -- where a group has one IK solver
1290  {
1291  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1292  if (jt != allocators.end())
1293  {
1294  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1295  result.first = jt->second;
1296  jmg->setSolverAllocators(result);
1297  }
1298  }
1299 
1300  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1301  // setSolverAllocators()
1302  for (JointModelGroup* jmg : joint_model_groups_)
1303  {
1304  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> result;
1305  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1306  if (jt == allocators.end())
1307  {
1308  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1309  std::set<const JointModel*> joints;
1310  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1311 
1312  std::vector<const JointModelGroup*> subs;
1313 
1314  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1315  for (std::map<std::string, SolverAllocatorFn>::const_iterator kt = allocators.begin(); kt != allocators.end();
1316  ++kt)
1317  {
1318  const JointModelGroup* sub = getJointModelGroup(kt->first);
1319  if (!sub) // this should actually not happen, all groups should be well defined
1320  {
1321  subs.clear();
1322  break;
1323  }
1324  std::set<const JointModel*> sub_joints;
1325  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1326 
1327  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1328  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1329  std::set<const JointModel*> resultj;
1330  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1331  std::inserter(resultj, resultj.end()));
1332  // TODO: instead of maintaining disjoint joint sets here,
1333  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1334  // There, a disjoint bijection from joints to solvers is computed anyway.
1335  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1336  // But, if the overlap only involves fixed joints, we could consider all sub groups
1337  subs.push_back(sub);
1338  joints.swap(resultj);
1339  }
1340  }
1341 
1342  // if we found subgroups, pass that information to the planning group
1343  if (!subs.empty())
1344  {
1345  std::stringstream ss;
1346  for (std::size_t i = 0; i < subs.size(); ++i)
1347  {
1348  ss << subs[i]->getName() << " ";
1349  result.second[subs[i]] = allocators.find(subs[i]->getName())->second;
1350  }
1351  ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1352  ss.str().c_str());
1353  }
1354  jmg->setSolverAllocators(result);
1355  }
1356  }
1357 }
1358 
1359 void RobotModel::printModelInfo(std::ostream& out) const
1360 {
1361  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
1362  << std::endl;
1363 
1364  std::ios_base::fmtflags old_flags = out.flags();
1365  out.setf(std::ios::fixed, std::ios::floatfield);
1366  std::streamsize old_prec = out.precision();
1367  out.precision(5);
1368  out << "Joints: " << std::endl;
1369  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
1370  {
1371  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
1372  << std::endl;
1373  out << " * Joint Index: " << joint_model_vector_[i]->getJointIndex() << std::endl;
1374  const std::vector<std::string>& vn = joint_model_vector_[i]->getVariableNames();
1375  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
1376  << std::endl;
1377  int idx = joint_model_vector_[i]->getFirstVariableIndex();
1378  for (std::vector<std::string>::const_iterator it = vn.begin(); it != vn.end(); ++it)
1379  {
1380  out << " * '" << *it << "', index " << idx++ << " in full state";
1381  if (joint_model_vector_[i]->getMimic())
1382  out << ", mimic '" << joint_model_vector_[i]->getMimic()->getName() << "'";
1383  if (joint_model_vector_[i]->isPassive())
1384  out << ", passive";
1385  out << std::endl;
1386  out << " " << joint_model_vector_[i]->getVariableBounds(*it) << std::endl;
1387  }
1388  }
1389  out << std::endl;
1390  out.precision(old_prec);
1391  out.flags(old_flags);
1392  out << "Links: " << std::endl;
1393  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
1394  {
1395  out << " '" << link_model_vector_[i]->getName() << "' with " << link_model_vector_[i]->getShapes().size()
1396  << " geoms" << std::endl;
1397  if (link_model_vector_[i]->parentJointIsFixed())
1398  out << " * "
1399  << "parent joint is fixed" << std::endl;
1400  if (link_model_vector_[i]->jointOriginTransformIsIdentity())
1401  out << " * "
1402  << "joint origin transform is identity" << std::endl;
1403  }
1404 
1405  out << "Available groups: " << std::endl;
1406  for (std::size_t i = 0; i < joint_model_groups_.size(); ++i)
1407  joint_model_groups_[i]->printGroupInfo(out);
1408 }
1409 
1410 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
1411  LinkTransformMap& associated_transforms)
1412 {
1413  associated_transforms[link] = transform * link->getJointOriginTransform();
1414  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1415  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1416  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1417  transform * link->getJointOriginTransform(), associated_transforms);
1418 }
1419 
1420 } // end of namespace core
1421 } // end of namespace moveit
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:514
void setVisualMesh(const std::string &visual_mesh, const Eigen::Isometry3d &origin, const Eigen::Vector3d &scale)
Definition: link_model.cpp:115
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:470
JointModel * constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model)
Given a urdf joint model, a child link and a set of virtual joints, build up the corresponding JointM...
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:549
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:476
void setEndEffectorParent(const std::string &group, const std::string &link)
If this group is an end-effector, specify the parent group (e.g., the arm holding the eef) and the li...
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:473
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:546
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints...
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:467
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:54
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:464
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:557
double distance(const double *state1, const double *state2) const
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:62
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:77
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:82
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:451
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:541
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:72
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:554
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:536
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:490
std::vector< const JointModel * > joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:499
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:457
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:563
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
const std::vector< const JointModel * > & 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:511
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:516
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:569
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:505
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:485
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:493
Mesh * createMeshFromResource(const std::string &resource)
const 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:502
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:459
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:455
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:496
std::vector< std::string > variable_names_
The names of the DOF that make up this state (this is just a sequence of joint variable names; not ne...
Definition: robot_model.h:533
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:543
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:518
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:560
Main namespace for MoveIt!
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:566
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:508
void buildGroupStates(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build the default states defined in the SRDF...
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:479
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for thw joints is stored.
Definition: robot_model.h:527
JointModel * buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model)
(This function is mostly intended for internal use). Given a parent link, build up (recursively)...
void setVariableBounds(const std::string &variable, const VariableBounds &bounds)
Set the lower and upper bounds for a variable. Throw an exception if the variable was not found...
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:482
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 Tue Sep 8 2020 04:12:45