moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 
39 #include <geometric_shapes/shape_operations.h>
40 #include <rclcpp/logger.hpp>
41 #include <algorithm>
42 #include <limits>
43 #include <cmath>
44 #include <memory>
45 
47 
48 namespace moveit
49 {
50 namespace core
51 {
52 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_robot_model.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 (std::pair<const std::string, JointModelGroup*>& it : joint_model_group_map_)
65  delete it.second;
66  for (JointModel* joint_model : joint_model_vector_)
67  delete joint_model;
68  for (LinkModel* link_model : link_model_vector_)
69  delete link_model;
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 {
84  root_joint_ = nullptr;
85  root_link_ = nullptr;
87  variable_count_ = 0;
88  model_name_ = urdf_model.getName();
89  RCLCPP_INFO(LOGGER, "Loading robot model '%s'...", model_name_.c_str());
90 
91  if (urdf_model.getRoot())
92  {
93  const urdf::Link* root_link_ptr = urdf_model.getRoot().get();
94  model_frame_ = root_link_ptr->name;
95 
96  RCLCPP_DEBUG(LOGGER, "... building kinematic chain");
97  root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model);
98  if (root_joint_)
100  RCLCPP_DEBUG(LOGGER, "... building mimic joints");
101  buildMimic(urdf_model);
102 
103  RCLCPP_DEBUG(LOGGER, "... computing joint indexing");
104  buildJointInfo();
105 
107  {
108  RCLCPP_WARN(LOGGER, "No geometry is associated to any robot links");
109  }
110 
111  // build groups
112 
113  RCLCPP_DEBUG(LOGGER, "... constructing joint groups");
114  buildGroups(srdf_model);
115 
116  RCLCPP_DEBUG(LOGGER, "... constructing joint group states");
117  buildGroupStates(srdf_model);
118 
119  // For debugging entire model
120  // printModelInfo(std::cout);
121  }
122  else
123  {
124  RCLCPP_WARN(LOGGER, "No root link found");
125  }
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 (const JointModel* parent : parents)
144  descendants[parent].second.insert(joint);
145 
146  const LinkModel* lm = joint->getChildLinkModel();
147  if (!lm)
148  return;
149 
150  for (const JointModel* parent : parents)
151  descendants[parent].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 (const JointModel* child_joint_model : ch)
157  computeDescendantsHelper(child_joint_model, parents, seen, descendants);
158  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
159  for (const JointModel* mimic_joint_model : mim)
160  computeDescendantsHelper(mimic_joint_model, 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 (const JointModel* m : b)
180  common_roots[ch[i]->getJointIndex() * size + m->getJointIndex()] =
181  common_roots[ch[i]->getJointIndex() + m->getJointIndex() * size] = joint->getJointIndex();
182  for (const JointModel* k : a)
183  {
184  common_roots[k->getJointIndex() * size + ch[j]->getJointIndex()] =
185  common_roots[k->getJointIndex() + ch[j]->getJointIndex() * size] = joint->getJointIndex();
186  for (const JointModel* m : b)
187  common_roots[k->getJointIndex() * size + m->getJointIndex()] =
188  common_roots[k->getJointIndex() + m->getJointIndex() * size] = joint->getJointIndex();
189  }
190  }
191  computeCommonRootsHelper(ch[i], common_roots, size);
192  }
193 }
194 } // namespace
195 
196 void RobotModel::computeCommonRoots()
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 (const JointModel* joint_model : joint_model_vector_)
213  {
214  // the common root of a joint and itself is the same joint:
215  common_joint_roots_[joint_model->getJointIndex() * (1 + joint_model_vector_.size())] = joint_model->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->getDescendantJointModels();
219  for (const JointModel* descendant_joint_model : d)
220  common_joint_roots_[descendant_joint_model->getJointIndex() * joint_model_vector_.size() +
221  joint_model->getJointIndex()] =
222  common_joint_roots_[descendant_joint_model->getJointIndex() +
223  joint_model->getJointIndex() * joint_model_vector_.size()] = joint_model->getJointIndex();
224  }
225 }
226 
227 void RobotModel::computeDescendants()
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 (std::pair<const JointModel* const, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
236  std::set<const JointModel*, OrderJointsByIndex>>>& descendant :
237  descendants)
238  {
239  JointModel* jm = const_cast<JointModel*>(descendant.first);
240  for (const JointModel* jt : descendant.second.second)
241  jm->addDescendantJointModel(jt);
242  for (const LinkModel* jt : descendant.second.first)
243  jm->addDescendantLinkModel(jt);
244  }
245 }
246 
247 void RobotModel::buildJointInfo()
248 {
249  // construct additional maps for easy access by name
250  variable_count_ = 0;
252  variable_names_.reserve(joint_model_vector_.size());
254 
255  for (const auto& joint : joint_model_vector_)
256  {
257  const std::vector<std::string>& name_order = joint->getVariableNames();
258 
259  // compute index map
260  if (!name_order.empty())
261  {
262  for (std::size_t j = 0; j < name_order.size(); ++j)
263  {
264  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
265  variable_names_.push_back(name_order[j]);
266  joints_of_variable_.push_back(joint);
267  }
268  if (joint->getMimic() == nullptr)
269  {
271  active_joint_model_vector_.push_back(joint);
272  active_joint_model_names_vector_.push_back(joint->getName());
273  active_joint_model_vector_const_.push_back(joint);
274  active_joint_models_bounds_.push_back(&joint->getVariableBounds());
275  }
276 
277  if (joint->getType() == JointModel::REVOLUTE && static_cast<const RevoluteJointModel*>(joint)->isContinuous())
278  continuous_joint_model_vector_.push_back(joint);
279 
280  joint_variables_index_map_[joint->getName()] = variable_count_;
281 
282  // compute variable count
283  std::size_t vc = joint->getVariableCount();
284  variable_count_ += vc;
285  if (vc == 1)
286  single_dof_joints_.push_back(joint);
287  else
288  multi_dof_joints_.push_back(joint);
289  }
290  }
291 
292  std::vector<bool> link_considered(link_model_vector_.size(), false);
293  for (const LinkModel* link : link_model_vector_)
294  {
295  if (link_considered[link->getLinkIndex()])
296  continue;
297 
298  LinkTransformMap associated_transforms;
299  computeFixedTransforms(link, link->getJointOriginTransform().inverse(), associated_transforms);
300  for (auto& tf_base : associated_transforms)
301  {
302  link_considered[tf_base.first->getLinkIndex()] = true;
303  for (auto& tf_target : associated_transforms)
304  {
305  if (&tf_base != &tf_target)
306  const_cast<LinkModel*>(tf_base.first) // regain write access to base LinkModel*
307  ->addAssociatedFixedTransform(tf_target.first, tf_base.second.inverse() * tf_target.second);
308  }
309  }
310  }
311 
312  computeDescendants();
313  computeCommonRoots(); // must be called _after_ list of descendants was computed
314 }
315 
316 void RobotModel::buildGroupStates(const srdf::Model& srdf_model)
317 {
318  // copy the default states to the groups
319  const std::vector<srdf::Model::GroupState>& ds = srdf_model.getGroupStates();
320  for (const srdf::Model::GroupState& group_state : ds)
321  {
322  if (hasJointModelGroup(group_state.group_))
323  {
324  JointModelGroup* jmg = getJointModelGroup(group_state.group_);
325  std::vector<const JointModel*> remaining_joints = jmg->getActiveJointModels();
326  std::map<std::string, double> state;
327  for (std::map<std::string, std::vector<double>>::const_iterator jt = group_state.joint_values_.begin();
328  jt != group_state.joint_values_.end(); ++jt)
329  {
330  if (jmg->hasJointModel(jt->first))
331  {
332  const JointModel* jm = jmg->getJointModel(jt->first);
333  const std::vector<std::string>& vn = jm->getVariableNames();
334  // Remove current joint name from remaining list.
335  auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm);
336  if (it_found != remaining_joints.end())
337  remaining_joints.erase(it_found);
338  if (vn.size() == jt->second.size())
339  for (std::size_t j = 0; j < vn.size(); ++j)
340  state[vn[j]] = jt->second[j];
341  else
342  RCLCPP_ERROR(LOGGER,
343  "The model for joint '%s' requires %d variable values, "
344  "but only %d variable values were supplied in default state '%s' for group '%s'",
345  jt->first.c_str(), static_cast<int>(vn.size()), static_cast<int>(jt->second.size()),
346  group_state.name_.c_str(), jmg->getName().c_str());
347  }
348  else
349  RCLCPP_ERROR(LOGGER,
350  "Group state '%s' specifies value for joint '%s', "
351  "but that joint is not part of group '%s'",
352  group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
353  }
354  if (!remaining_joints.empty())
355  {
356  std::stringstream missing;
357  missing << (*remaining_joints.begin())->getName();
358  for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); ++j)
359  {
360  missing << ", " << (*j)->getName();
361  }
362  RCLCPP_WARN_STREAM(LOGGER, "Group state '" << group_state.name_
363  << "' doesn't specify all group joints in group '"
364  << group_state.group_ << "'. " << missing.str() << " "
365  << (remaining_joints.size() > 1 ? "are" : "is") << " missing.");
366  }
367  if (!state.empty())
368  jmg->addDefaultState(group_state.name_, state);
369  }
370  else
371  RCLCPP_ERROR(LOGGER, "Group state '%s' specified for group '%s', but that group does not exist",
372  group_state.name_.c_str(), group_state.group_.c_str());
373  }
374 }
375 
376 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
377 {
378  // compute mimic joints
379  for (JointModel* joint_model : joint_model_vector_)
380  {
381  const urdf::Joint* jm = urdf_model.getJoint(joint_model->getName()).get();
382  if (jm)
383  if (jm->mimic)
384  {
385  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
386  if (jit != joint_model_map_.end())
387  {
388  if (joint_model->getVariableCount() == jit->second->getVariableCount())
389  joint_model->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
390  else
391  RCLCPP_ERROR(LOGGER, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
392  joint_model->getName().c_str(), jm->mimic->joint_name.c_str());
393  }
394  else
395  RCLCPP_ERROR(LOGGER, "Joint '%s' cannot mimic unknown joint '%s'", joint_model->getName().c_str(),
396  jm->mimic->joint_name.c_str());
397  }
398  }
399 
400  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
401  bool change = true;
402  while (change)
403  {
404  change = false;
405  for (JointModel* joint_model : joint_model_vector_)
406  if (joint_model->getMimic())
407  {
408  if (joint_model->getMimic()->getMimic())
409  {
410  joint_model->setMimic(joint_model->getMimic()->getMimic(),
411  joint_model->getMimicFactor() * joint_model->getMimic()->getMimicFactor(),
412  joint_model->getMimicOffset() +
413  joint_model->getMimicFactor() * joint_model->getMimic()->getMimicOffset());
414  change = true;
415  }
416  if (joint_model == joint_model->getMimic())
417  {
418  RCLCPP_ERROR(LOGGER, "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
419  for (JointModel* joint_model_recal : joint_model_vector_)
420  joint_model_recal->setMimic(nullptr, 0.0, 0.0);
421  change = false;
422  break;
423  }
424  }
425  }
426  // build mimic requests
427  for (JointModel* joint_model : joint_model_vector_)
428  if (joint_model->getMimic())
429  {
430  const_cast<JointModel*>(joint_model->getMimic())->addMimicRequest(joint_model);
431  mimic_joints_.push_back(joint_model);
432  }
433 }
434 
435 bool RobotModel::hasEndEffector(const std::string& eef) const
436 {
437  return end_effectors_map_.find(eef) != end_effectors_map_.end();
438 }
439 
440 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
441 {
442  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
443  if (it == end_effectors_map_.end())
444  {
445  it = joint_model_group_map_.find(name);
446  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
447  return it->second;
448  RCLCPP_ERROR(LOGGER, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
449  return nullptr;
450  }
451  return it->second;
452 }
453 
455 {
456  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
457  if (it == end_effectors_map_.end())
458  {
459  it = joint_model_group_map_.find(name);
460  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
461  return it->second;
462  RCLCPP_ERROR(LOGGER, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
463  return nullptr;
464  }
465  return it->second;
466 }
467 
468 bool RobotModel::hasJointModelGroup(const std::string& name) const
469 {
470  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
471 }
472 
473 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
474 {
475  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
476  if (it == joint_model_group_map_.end())
477  {
478  RCLCPP_ERROR(LOGGER, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
479  return nullptr;
480  }
481  return it->second;
482 }
483 
485 {
486  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
487  if (it == joint_model_group_map_.end())
488  {
489  RCLCPP_ERROR(LOGGER, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
490  return nullptr;
491  }
492  return it->second;
493 }
494 
495 void RobotModel::buildGroups(const srdf::Model& srdf_model)
496 {
497  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
498 
499  // the only thing tricky is dealing with subgroups
500  std::vector<bool> processed(group_configs.size(), false);
501 
502  bool added = true;
503  while (added)
504  {
505  added = false;
506 
507  // going to make passes until we can't do anything else
508  for (std::size_t i = 0; i < group_configs.size(); ++i)
509  if (!processed[i])
510  {
511  // if we haven't processed, check and see if the dependencies are met yet
512  bool all_subgroups_added = true;
513  for (const std::string& subgroup : group_configs[i].subgroups_)
514  if (joint_model_group_map_.find(subgroup) == joint_model_group_map_.end())
515  {
516  all_subgroups_added = false;
517  break;
518  }
519  if (all_subgroups_added)
520  {
521  added = true;
522  processed[i] = true;
523  if (!addJointModelGroup(group_configs[i]))
524  {
525  RCLCPP_WARN(LOGGER, "Failed to add group '%s'", group_configs[i].name_.c_str());
526  }
527  }
528  }
529  }
530 
531  for (std::size_t i = 0; i < processed.size(); ++i)
532  if (!processed[i])
533  {
534  RCLCPP_WARN(LOGGER, "Could not process group '%s' due to unmet subgroup dependencies",
535  group_configs[i].name_.c_str());
536  }
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 (JointModelGroup* joint_model_group : joint_model_groups_)
542  {
543  joint_model_groups_const_.push_back(joint_model_group);
544  joint_model_group_names_.push_back(joint_model_group->getName());
545  }
546 
547  buildGroupsInfoSubgroups();
548  buildGroupsInfoEndEffectors(srdf_model);
549 }
550 
551 void RobotModel::buildGroupsInfoSubgroups()
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 (const JointModel* sub_joint : sub_joints)
567  if (joints.find(sub_joint) == 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 
580 void RobotModel::buildGroupsInfoEndEffectors(const srdf::Model& srdf_model)
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 (const srdf::Model::EndEffector& eef : eefs)
588  if (eef.component_group_ == it->first)
589  {
590  // if it is, mark it as such
591  it->second->setEndEffectorName(eef.name_);
592  end_effectors_map_[eef.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(eef.parent_link_))
603  {
604  jt->second->attachEndEffector(eef.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 (!eef.parent_group_.empty())
612  {
613  JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eef.parent_group_);
614  if (jt != joint_model_group_map_.end())
615  {
616  if (jt->second->hasLinkModel(eef.parent_link_))
617  {
618  if (jt->second != it->second)
619  eef_parent_group = jt->second;
620  else
621  RCLCPP_ERROR(LOGGER, "Group '%s' for end-effector '%s' cannot be its own parent",
622  eef.parent_group_.c_str(), eef.name_.c_str());
623  }
624  else
625  RCLCPP_ERROR(LOGGER,
626  "Group '%s' was specified as parent group for end-effector '%s' "
627  "but it does not include the parent link '%s'",
628  eef.parent_group_.c_str(), eef.name_.c_str(), eef.parent_link_.c_str());
629  }
630  else
631  RCLCPP_ERROR(LOGGER, "Group name '%s' not found (specified as parent group for end-effector '%s')",
632  eef.parent_group_.c_str(), eef.name_.c_str());
633  }
634 
635  // if no parent group was specified, use a default one
636  if (eef_parent_group == nullptr)
637  if (!possible_parent_groups.empty())
638  {
639  // if there are multiple options for the group that contains this end-effector,
640  // we pick the group with fewest joints.
641  std::size_t best = 0;
642  for (std::size_t g = 1; g < possible_parent_groups.size(); ++g)
643  if (possible_parent_groups[g]->getJointModels().size() <
644  possible_parent_groups[best]->getJointModels().size())
645  best = g;
646  eef_parent_group = possible_parent_groups[best];
647  }
648 
649  if (eef_parent_group)
650  {
651  it->second->setEndEffectorParent(eef_parent_group->getName(), eef.parent_link_);
652  }
653  else
654  {
655  RCLCPP_WARN(LOGGER, "Could not identify parent group for end-effector '%s'", eef.name_.c_str());
656  it->second->setEndEffectorParent("", eef.parent_link_);
657  }
658  }
659  }
660  std::sort(end_effectors_.begin(), end_effectors_.end(), OrderGroupsByName());
661 }
662 
663 bool RobotModel::addJointModelGroup(const srdf::Model::Group& gc)
664 {
665  if (joint_model_group_map_.find(gc.name_) != joint_model_group_map_.end())
666  {
667  RCLCPP_WARN(LOGGER, "A group named '%s' already exists. Not adding.", gc.name_.c_str());
668  return false;
669  }
670 
671  std::set<const JointModel*> jset;
672 
673  // add joints from chains
674  for (const std::pair<std::string, std::string>& chain : gc.chains_)
675  {
676  const LinkModel* base_link = getLinkModel(chain.first);
677  const LinkModel* tip_link = getLinkModel(chain.second);
678  if (base_link && tip_link)
679  {
680  // go from tip, up the chain, until we hit the root or we find the base_link
681  const LinkModel* lm = tip_link;
682  std::vector<const JointModel*> cj;
683  while (lm)
684  {
685  if (lm == base_link)
686  break;
687  cj.push_back(lm->getParentJointModel());
688  lm = lm->getParentJointModel()->getParentLinkModel();
689  }
690  // if we did not find the base_link, we could have a chain like e.g.,
691  // from one end-effector to another end-effector, so the root is in between
692  if (lm != base_link)
693  {
694  // we go up the chain from the base this time, and see where we intersect the other chain
695  lm = base_link;
696  std::size_t index = 0;
697  std::vector<const JointModel*> cj2;
698  while (lm)
699  {
700  for (std::size_t j = 0; j < cj.size(); ++j)
701  if (cj[j] == lm->getParentJointModel())
702  {
703  index = j + 1;
704  break;
705  }
706  if (index > 0)
707  break;
708  cj2.push_back(lm->getParentJointModel());
709  lm = lm->getParentJointModel()->getParentLinkModel();
710  }
711  if (index > 0)
712  {
713  jset.insert(cj.begin(), cj.begin() + index);
714  jset.insert(cj2.begin(), cj2.end());
715  }
716  }
717  else
718  // if we have a simple chain, just add the joints
719  jset.insert(cj.begin(), cj.end());
720  }
721  }
722 
723  // add joints
724  for (const std::string& joint : gc.joints_)
725  {
726  const JointModel* j = getJointModel(joint);
727  if (j)
728  jset.insert(j);
729  }
730 
731  // add joints that are parents of included links
732  for (const std::string& link : gc.links_)
733  {
734  const LinkModel* l = getLinkModel(link);
735  if (l)
736  jset.insert(l->getParentJointModel());
737  }
738 
739  // add joints from subgroups
740  for (const std::string& subgroup : gc.subgroups_)
741  {
742  const JointModelGroup* sg = getJointModelGroup(subgroup);
743  if (sg)
744  {
745  // active joints
746  const std::vector<const JointModel*>& js = sg->getJointModels();
747  for (const JointModel* j : js)
748  jset.insert(j);
749 
750  // fixed joints
751  const std::vector<const JointModel*>& fs = sg->getFixedJointModels();
752  for (const JointModel* f : fs)
753  jset.insert(f);
754 
755  // mimic joints
756  const std::vector<const JointModel*>& ms = sg->getMimicJointModels();
757  for (const JointModel* m : ms)
758  jset.insert(m);
759  }
760  }
761 
762  if (jset.empty())
763  {
764  RCLCPP_WARN(LOGGER, "Group '%s' must have at least one valid joint", gc.name_.c_str());
765  return false;
766  }
767 
768  std::vector<const JointModel*> joints;
769  joints.reserve(jset.size());
770  for (const JointModel* it : jset)
771  joints.push_back(it);
772 
773  JointModelGroup* jmg = new JointModelGroup(gc.name_, gc, joints, this);
774  joint_model_group_map_[gc.name_] = jmg;
775 
776  return true;
777 }
778 
779 JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, const srdf::Model& srdf_model)
780 {
781  // construct the joint
782  JointModel* joint = constructJointModel(urdf_link, srdf_model);
783 
784  if (joint == nullptr)
785  return nullptr;
786 
787  // bookkeeping for the joint
788  joint_model_vector_.push_back(joint);
789  joint_model_map_[joint->getName()] = 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_model_vector_.push_back(link);
802  link_model_vector_const_.push_back(link);
803  link_model_names_vector_.push_back(link->getName());
804  if (!link->getShapes().empty())
805  {
807  link_model_names_with_collision_geometry_vector_.push_back(link->getName());
808  link->setFirstCollisionBodyTransformIndex(link_geometry_count_);
809  link_geometry_count_ += link->getShapes().size();
810  }
811  link->setParentJointModel(joint);
812 
813  // recursively build child links (and joints)
814  for (const urdf::LinkSharedPtr& child_link : urdf_link->child_links)
815  {
816  JointModel* jm = buildRecursive(link, child_link.get(), srdf_model);
817  if (jm)
818  link->addChildJointModel(jm);
819  }
820  return joint;
821 }
822 
823 namespace
824 {
825 // construct bounds for 1DOF joint
826 static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
827 {
828  VariableBounds b;
829  if (urdf_joint->safety)
830  {
831  b.position_bounded_ = true;
832  b.min_position_ = urdf_joint->safety->soft_lower_limit;
833  b.max_position_ = urdf_joint->safety->soft_upper_limit;
834  if (urdf_joint->limits)
835  {
836  if (urdf_joint->limits->lower > b.min_position_)
837  b.min_position_ = urdf_joint->limits->lower;
838  if (urdf_joint->limits->upper < b.max_position_)
839  b.max_position_ = urdf_joint->limits->upper;
840  }
841  }
842  else
843  {
844  if (urdf_joint->limits)
845  {
846  b.position_bounded_ = true;
847  b.min_position_ = urdf_joint->limits->lower;
848  b.max_position_ = urdf_joint->limits->upper;
849  }
850  }
851  if (urdf_joint->limits)
852  {
853  b.max_velocity_ = fabs(urdf_joint->limits->velocity);
854  b.min_velocity_ = -b.max_velocity_;
855  b.velocity_bounded_ = b.max_velocity_ > std::numeric_limits<double>::epsilon();
856  }
857  return b;
858 }
859 } // namespace
860 
861 JointModel* RobotModel::constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model)
862 {
863  JointModel* new_joint_model = nullptr;
864  auto parent_joint = child_link->parent_joint ? child_link->parent_joint.get() : nullptr;
865  auto joint_index = joint_model_vector_.size();
866  auto first_variable_index = joint_model_vector_.empty() ? 0 :
867  joint_model_vector_.back()->getFirstVariableIndex() +
868  joint_model_vector_.back()->getVariableCount();
869 
870  // if parent_joint exists, must be the root link transform
871  if (parent_joint)
872  {
873  switch (parent_joint->type)
874  {
875  case urdf::Joint::REVOLUTE:
876  {
877  RevoluteJointModel* j = new RevoluteJointModel(parent_joint->name, joint_index, first_variable_index);
878  j->setVariableBounds(j->getName(), jointBoundsFromURDF(parent_joint));
879  j->setContinuous(false);
880  j->setAxis(Eigen::Vector3d(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z));
881  new_joint_model = j;
882  }
883  break;
884  case urdf::Joint::CONTINUOUS:
885  {
886  RevoluteJointModel* j = new RevoluteJointModel(parent_joint->name, joint_index, first_variable_index);
887  j->setVariableBounds(j->getName(), jointBoundsFromURDF(parent_joint));
888  j->setContinuous(true);
889  j->setAxis(Eigen::Vector3d(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z));
890  new_joint_model = j;
891  }
892  break;
893  case urdf::Joint::PRISMATIC:
894  {
895  PrismaticJointModel* j = new PrismaticJointModel(parent_joint->name, joint_index, first_variable_index);
896  j->setVariableBounds(j->getName(), jointBoundsFromURDF(parent_joint));
897  j->setAxis(Eigen::Vector3d(parent_joint->axis.x, parent_joint->axis.y, parent_joint->axis.z));
898  new_joint_model = j;
899  }
900  break;
901  case urdf::Joint::FLOATING:
902  new_joint_model = new FloatingJointModel(parent_joint->name, joint_index, first_variable_index);
903  break;
904  case urdf::Joint::PLANAR:
905  new_joint_model = new PlanarJointModel(parent_joint->name, joint_index, first_variable_index);
906  break;
907  case urdf::Joint::FIXED:
908  new_joint_model = new FixedJointModel(parent_joint->name, joint_index, first_variable_index);
909  break;
910  default:
911  RCLCPP_ERROR(LOGGER, "Unknown joint type: %d", static_cast<int>(parent_joint->type));
912  break;
913  }
914  }
915  else // if parent_joint passed in as null, then we're at root of URDF model
916  {
917  const std::vector<srdf::Model::VirtualJoint>& virtual_joints = srdf_model.getVirtualJoints();
918  for (const srdf::Model::VirtualJoint& virtual_joint : virtual_joints)
919  {
920  if (virtual_joint.child_link_ != child_link->name)
921  {
922  if (child_link->name == "world" && virtual_joint.type_ == "fixed" && child_link->collision_array.empty() &&
923  !child_link->collision && child_link->visual_array.empty() && !child_link->visual)
924  // Gazebo requires a fixed link from a dummy world link to the first robot's link
925  // Skip warning in this case and create a fixed joint with given name
926  new_joint_model = new FixedJointModel(virtual_joint.name_, joint_index, first_variable_index);
927  else
928  RCLCPP_WARN(LOGGER,
929  "Skipping virtual joint '%s' because its child frame '%s' "
930  "does not match the URDF frame '%s'",
931  virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str());
932  }
933  else if (virtual_joint.parent_frame_.empty())
934  {
935  RCLCPP_WARN(LOGGER, "Skipping virtual joint '%s' because its parent frame is empty",
936  virtual_joint.name_.c_str());
937  }
938  else
939  {
940  if (virtual_joint.type_ == "fixed")
941  new_joint_model = new FixedJointModel(virtual_joint.name_, joint_index, first_variable_index);
942  else if (virtual_joint.type_ == "planar")
943  new_joint_model = new PlanarJointModel(virtual_joint.name_, joint_index, first_variable_index);
944  else if (virtual_joint.type_ == "floating")
945  new_joint_model = new FloatingJointModel(virtual_joint.name_, joint_index, first_variable_index);
946  if (new_joint_model)
947  {
948  // for fixed frames we still use the robot root link
949  if (virtual_joint.type_ != "fixed")
950  {
951  model_frame_ = virtual_joint.parent_frame_;
952  }
953  break;
954  }
955  }
956  }
957  if (!new_joint_model)
958  {
959  RCLCPP_INFO(LOGGER, "No root/virtual joint specified in SRDF. Assuming fixed joint");
960  new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT", joint_index, first_variable_index);
961  }
962  }
963 
964  if (new_joint_model)
965  {
966  new_joint_model->setDistanceFactor(new_joint_model->getStateSpaceDimension());
967  const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
968  for (const srdf::Model::PassiveJoint& pjoint : pjoints)
969  {
970  if (new_joint_model->getName() == pjoint.name_)
971  {
972  new_joint_model->setPassive(true);
973  break;
974  }
975  }
976 
977  for (const srdf::Model::JointProperty& property : srdf_model.getJointProperties(new_joint_model->getName()))
978  {
979  if (property.property_name_ == "angular_distance_weight")
980  {
981  double angular_distance_weight;
982  try
983  {
984  std::string::size_type sz;
985  angular_distance_weight = std::stod(property.value_, &sz);
986  if (sz != property.value_.size())
987  {
988  RCLCPP_WARN_STREAM(LOGGER, "Extra characters after property " << property.property_name_ << " for joint "
989  << property.joint_name_ << " as double: '"
990  << property.value_.substr(sz) << "'");
991  }
992  }
993  catch (const std::invalid_argument& e)
994  {
995  RCLCPP_ERROR_STREAM(LOGGER, "Unable to parse property " << property.property_name_ << " for joint "
996  << property.joint_name_ << " as double: '"
997  << property.value_ << "'");
998  continue;
999  }
1000 
1001  if (new_joint_model->getType() == JointModel::JointType::PLANAR)
1002  {
1003  ((PlanarJointModel*)new_joint_model)->setAngularDistanceWeight(angular_distance_weight);
1004  }
1005  else if (new_joint_model->getType() == JointModel::JointType::FLOATING)
1006  {
1007  ((FloatingJointModel*)new_joint_model)->setAngularDistanceWeight(angular_distance_weight);
1008  }
1009  else
1010  {
1011  RCLCPP_ERROR_STREAM(LOGGER, "Cannot apply property " << property.property_name_
1012  << " to joint type: " << new_joint_model->getTypeName());
1013  }
1014  }
1015  else if (property.property_name_ == "motion_model")
1016  {
1017  if (new_joint_model->getType() != JointModel::JointType::PLANAR)
1018  {
1019  RCLCPP_ERROR(LOGGER, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(),
1020  new_joint_model->getTypeName().c_str());
1021  continue;
1022  }
1023 
1024  PlanarJointModel::MotionModel motion_model;
1025  if (property.value_ == "holonomic")
1026  {
1027  motion_model = PlanarJointModel::MotionModel::HOLONOMIC;
1028  }
1029  else if (property.value_ == "diff_drive")
1030  {
1031  motion_model = PlanarJointModel::MotionModel::DIFF_DRIVE;
1032  }
1033  else
1034  {
1035  RCLCPP_ERROR_STREAM(LOGGER, "Unknown value for property " << property.property_name_ << " ("
1036  << property.joint_name_ << "): '" << property.value_
1037  << "'");
1038  RCLCPP_ERROR(LOGGER, "Valid values are 'holonomic' and 'diff_drive'");
1039  continue;
1040  }
1041 
1042  ((PlanarJointModel*)new_joint_model)->setMotionModel(motion_model);
1043  }
1044  else if (property.property_name_ == "min_translational_distance")
1045  {
1046  if (new_joint_model->getType() != JointModel::JointType::PLANAR)
1047  {
1048  RCLCPP_ERROR(LOGGER, "Cannot apply property %s to joint type: %s", property.property_name_.c_str(),
1049  new_joint_model->getTypeName().c_str());
1050  continue;
1051  }
1052  double min_translational_distance;
1053  try
1054  {
1055  std::string::size_type sz;
1056  min_translational_distance = std::stod(property.value_, &sz);
1057  if (sz != property.value_.size())
1058  {
1059  RCLCPP_WARN_STREAM(LOGGER, "Extra characters after property " << property.property_name_ << " for joint "
1060  << property.joint_name_ << " as double: '"
1061  << property.value_.substr(sz) << "'");
1062  }
1063  }
1064  catch (const std::invalid_argument& e)
1065  {
1066  RCLCPP_ERROR_STREAM(LOGGER, "Unable to parse property " << property.property_name_ << " for joint "
1067  << property.joint_name_ << " as double: '"
1068  << property.value_ << "'");
1069  continue;
1070  }
1071 
1072  ((PlanarJointModel*)new_joint_model)->setMinTranslationalDistance(min_translational_distance);
1073  }
1074  else
1075  {
1076  RCLCPP_ERROR(LOGGER, "Unknown joint property: %s", property.property_name_.c_str());
1077  }
1078  }
1079  }
1080 
1081  return new_joint_model;
1082 }
1083 
1084 namespace
1085 {
1086 static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose)
1087 {
1088  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
1089  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
1090  return af;
1091 }
1092 } // namespace
1093 
1094 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
1095 {
1096  auto link_index = link_model_vector_.size();
1097  LinkModel* new_link_model = new LinkModel(urdf_link->name, link_index);
1098 
1099  const std::vector<urdf::CollisionSharedPtr>& col_array =
1100  urdf_link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, urdf_link->collision) :
1101  urdf_link->collision_array;
1102 
1103  std::vector<shapes::ShapeConstPtr> shapes;
1104  EigenSTL::vector_Isometry3d poses;
1105 
1106  for (const urdf::CollisionSharedPtr& col : col_array)
1107  {
1108  if (col && col->geometry)
1109  {
1110  shapes::ShapeConstPtr s = constructShape(col->geometry.get());
1111  if (s)
1112  {
1113  shapes.push_back(s);
1114  poses.push_back(urdfPose2Isometry3d(col->origin));
1115  }
1116  }
1117  }
1118 
1119  // Should we warn that old (melodic) behaviour has changed, not copying visual to collision geometries anymore?
1120  bool warn_about_missing_collision = false;
1121  if (shapes.empty())
1122  {
1123  const auto& vis_array = urdf_link->visual_array.empty() ? std::vector<urdf::VisualSharedPtr>{ urdf_link->visual } :
1124  urdf_link->visual_array;
1125  for (const urdf::VisualSharedPtr& vis : vis_array)
1126  {
1127  if (vis && vis->geometry)
1128  warn_about_missing_collision = true;
1129  }
1130  }
1131  if (warn_about_missing_collision)
1132  {
1133  RCLCPP_WARN_STREAM(LOGGER, // TODO(henningkayser): use child namespace "empty_collision_geometry"
1134  "Link " << urdf_link->name
1135  << " has visual geometry but no collision geometry. "
1136  "Collision geometry will be left empty. "
1137  "Fix your URDF file by explicitly specifying collision geometry.");
1138  }
1139 
1140  new_link_model->setGeometry(shapes, poses);
1141 
1142  // figure out visual mesh (try visual urdf tag first, collision tag otherwise
1143  if (urdf_link->visual && urdf_link->visual->geometry)
1144  {
1145  if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
1146  {
1147  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
1148  if (!mesh->filename.empty())
1149  new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin),
1150  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1151  }
1152  }
1153  else if (urdf_link->collision && urdf_link->collision->geometry)
1154  {
1155  if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
1156  {
1157  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
1158  if (!mesh->filename.empty())
1159  new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin),
1160  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1161  }
1162  }
1163 
1164  if (urdf_link->parent_joint)
1165  new_link_model->setJointOriginTransform(
1166  urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
1167 
1168  return new_link_model;
1169 }
1170 
1171 shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom)
1172 {
1173  shapes::Shape* new_shape = nullptr;
1174  switch (geom->type)
1175  {
1176  case urdf::Geometry::SPHERE:
1177  new_shape = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
1178  break;
1179  case urdf::Geometry::BOX:
1180  {
1181  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
1182  new_shape = new shapes::Box(dim.x, dim.y, dim.z);
1183  }
1184  break;
1185  case urdf::Geometry::CYLINDER:
1186  new_shape = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
1187  static_cast<const urdf::Cylinder*>(geom)->length);
1188  break;
1189  case urdf::Geometry::MESH:
1190  {
1191  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
1192  if (!mesh->filename.empty())
1193  {
1194  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
1195  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
1196  new_shape = m;
1197  }
1198  }
1199  break;
1200  default:
1201  RCLCPP_ERROR(LOGGER, "Unknown geometry type: %d", static_cast<int>(geom->type));
1202  break;
1203  }
1204 
1205  return shapes::ShapePtr(new_shape);
1206 }
1207 
1208 bool RobotModel::hasJointModel(const std::string& name) const
1209 {
1210  return joint_model_map_.find(name) != joint_model_map_.end();
1211 }
1212 
1213 bool RobotModel::hasLinkModel(const std::string& name) const
1214 {
1215  return link_model_map_.find(name) != link_model_map_.end();
1216 }
1217 
1218 const JointModel* RobotModel::getJointModel(const std::string& name) const
1219 {
1220  JointModelMap::const_iterator it = joint_model_map_.find(name);
1221  if (it != joint_model_map_.end())
1222  return it->second;
1223  RCLCPP_ERROR(LOGGER, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1224  return nullptr;
1225 }
1226 
1227 const JointModel* RobotModel::getJointModel(size_t index) const
1228 {
1229  if (index >= joint_model_vector_.size())
1230  {
1231  RCLCPP_ERROR(LOGGER, "Joint index '%li' out of bounds of joints in model '%s'", index, model_name_.c_str());
1232  return nullptr;
1233  }
1234  assert(joint_model_vector_[index]->getJointIndex() == index);
1235  return joint_model_vector_[index];
1236 }
1237 
1239 {
1240  JointModelMap::const_iterator it = joint_model_map_.find(name);
1241  if (it != joint_model_map_.end())
1242  return it->second;
1243  RCLCPP_ERROR(LOGGER, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1244  return nullptr;
1245 }
1246 
1247 const LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) const
1248 {
1249  return const_cast<RobotModel*>(this)->getLinkModel(name, has_link);
1250 }
1251 
1252 const LinkModel* RobotModel::getLinkModel(size_t index) const
1253 {
1254  if (index >= link_model_vector_.size())
1255  {
1256  RCLCPP_ERROR(LOGGER, "Link index '%li' out of bounds of links in model '%s'", index, model_name_.c_str());
1257  return nullptr;
1258  }
1259  assert(link_model_vector_[index]->getLinkIndex() == index);
1260  return link_model_vector_[index];
1261 }
1262 
1263 LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
1264 {
1265  if (has_link)
1266  *has_link = true; // Start out optimistic
1267  LinkModelMap::const_iterator it = link_model_map_.find(name);
1268  if (it != link_model_map_.end())
1269  return it->second;
1270 
1271  if (has_link)
1272  *has_link = false; // Report failure via argument
1273  else // Otherwise print error
1274  RCLCPP_ERROR(LOGGER, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1275  return nullptr;
1276 }
1277 
1279 {
1280  if (!link)
1281  return link;
1282  const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
1283  const moveit::core::JointModel* joint = link->getParentJointModel();
1284 
1285  while (parent_link && joint->getType() == moveit::core::JointModel::FIXED)
1286  {
1287  link = parent_link;
1288  joint = link->getParentJointModel();
1289  parent_link = joint->getParentLinkModel();
1290  }
1291  return link;
1292 }
1293 
1294 void RobotModel::updateMimicJoints(double* values) const
1295 {
1296  for (const JointModel* mimic_joint : mimic_joints_)
1297  {
1298  int src = mimic_joint->getMimic()->getFirstVariableIndex();
1299  int dest = mimic_joint->getFirstVariableIndex();
1300  values[dest] = values[src] * mimic_joint->getMimicFactor() + mimic_joint->getMimicOffset();
1301  }
1302 }
1303 
1304 void RobotModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const
1305 {
1306  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1308  updateMimicJoints(values);
1309 }
1310 
1311 void RobotModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
1312  std::map<std::string, double>& values) const
1313 {
1314  std::vector<double> tmp(variable_count_);
1315  getVariableRandomPositions(rng, &tmp[0]);
1316  values.clear();
1317  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1318  values[variable_names_[i]] = tmp[i];
1319 }
1320 
1322 {
1323  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1325  updateMimicJoints(values);
1326 }
1327 
1328 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1329 {
1330  std::vector<double> tmp(variable_count_);
1331  getVariableDefaultPositions(&tmp[0]);
1332  values.clear();
1333  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1334  values[variable_names_[i]] = tmp[i];
1335 }
1336 
1337 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1338  std::vector<std::string>& missing_variables) const
1339 {
1340  missing_variables.clear();
1341  std::set<std::string> keys(variables.begin(), variables.end());
1342  for (const std::string& variable_name : variable_names_)
1343  if (keys.find(variable_name) == keys.end())
1344  if (getJointOfVariable(variable_name)->getMimic() == nullptr)
1345  missing_variables.push_back(variable_name);
1346 }
1347 
1348 size_t RobotModel::getVariableIndex(const std::string& variable) const
1349 {
1350  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1351  if (it == joint_variables_index_map_.end())
1352  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1353  return it->second;
1354 }
1355 
1356 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1357 {
1358  double max_distance = 0.0;
1359  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1360  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1361  active_joint_model_vector_[j]->getDistanceFactor();
1362  return max_distance;
1363 }
1364 
1365 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1366  double margin) const
1367 {
1368  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1369  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1371  *active_joint_bounds[i], margin))
1372  return false;
1373  return true;
1374 }
1375 
1376 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1377 {
1378  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1379  bool change = false;
1380  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1382  *active_joint_bounds[i]))
1383  change = true;
1384  if (change)
1385  updateMimicJoints(state);
1386  return change;
1387 }
1388 
1389 double RobotModel::distance(const double* state1, const double* state2) const
1390 {
1391  double d = 0.0;
1392  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1393  d += active_joint_model_vector_[i]->getDistanceFactor() *
1395  state2 + active_joint_model_start_index_[i]);
1396  return d;
1397 }
1398 
1399 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1400 {
1401  moveit::core::checkInterpolationParamBounds(LOGGER, t);
1402  // we interpolate values only for active joint models (non-mimic)
1403  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1406  state + active_joint_model_start_index_[i]);
1407  // now we update mimic as needed
1408  updateMimicJoints(state);
1409 }
1410 
1411 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1412 {
1413  // we first set all the "simple" allocators -- where a group has one IK solver
1415  {
1416  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1417  if (jt != allocators.end())
1418  {
1419  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> solver_allocator_pair;
1420  solver_allocator_pair.first = jt->second;
1421  jmg->setSolverAllocators(solver_allocator_pair);
1422  }
1423  }
1424 
1425  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1426  // setSolverAllocators()
1428  {
1429  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> solver_allocator_pair;
1430  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1431  if (jt == allocators.end())
1432  {
1433  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1434  std::set<const JointModel*> joints;
1435  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1436 
1437  std::vector<const JointModelGroup*> subs;
1438 
1439  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1440  for (const std::pair<const std::string, SolverAllocatorFn>& allocator : allocators)
1441  {
1442  const JointModelGroup* sub = getJointModelGroup(allocator.first);
1443  if (!sub) // this should actually not happen, all groups should be well defined
1444  {
1445  subs.clear();
1446  break;
1447  }
1448  std::set<const JointModel*> sub_joints;
1449  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1450 
1451  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1452  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1453  std::set<const JointModel*> joint_model_set;
1454  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1455  std::inserter(joint_model_set, joint_model_set.end()));
1456  // TODO: instead of maintaining disjoint joint sets here,
1457  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1458  // There, a disjoint bijection from joints to solvers is computed anyway.
1459  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1460  // But, if the overlap only involves fixed joints, we could consider all sub groups
1461  subs.push_back(sub);
1462  joints.swap(joint_model_set);
1463  }
1464  }
1465 
1466  // if we found subgroups, pass that information to the planning group
1467  if (!subs.empty())
1468  {
1469  std::stringstream ss;
1470  for (const JointModelGroup* sub : subs)
1471  {
1472  ss << sub->getName() << " ";
1473  solver_allocator_pair.second[sub] = allocators.find(sub->getName())->second;
1474  }
1475  RCLCPP_DEBUG(LOGGER, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1476  ss.str().c_str());
1477  }
1478  jmg->setSolverAllocators(solver_allocator_pair);
1479  }
1480  }
1481 }
1482 
1483 void RobotModel::printModelInfo(std::ostream& out) const
1484 {
1485  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables\n";
1486 
1487  std::ios_base::fmtflags old_flags = out.flags();
1488  out.setf(std::ios::fixed, std::ios::floatfield);
1489  std::streamsize old_prec = out.precision();
1490  out.precision(5);
1491  out << "Joints: \n";
1492  for (JointModel* joint_model : joint_model_vector_)
1493  {
1494  out << " '" << joint_model->getName() << "' (" << joint_model->getTypeName() << ")\n";
1495  out << " * Joint Index: " << joint_model->getJointIndex() << '\n';
1496  const std::vector<std::string>& vn = joint_model->getVariableNames();
1497  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:\n"));
1498  int idx = joint_model->getFirstVariableIndex();
1499  for (const std::string& it : vn)
1500  {
1501  out << " * '" << it << "', index " << idx++ << " in full state";
1502  if (joint_model->getMimic())
1503  out << ", mimic '" << joint_model->getMimic()->getName() << "'";
1504  if (joint_model->isPassive())
1505  out << ", passive";
1506  out << '\n';
1507  out << " " << joint_model->getVariableBounds(it) << '\n';
1508  }
1509  }
1510  out << '\n';
1511  out.precision(old_prec);
1512  out.flags(old_flags);
1513  out << "Links: \n";
1514  for (LinkModel* link_model : link_model_vector_)
1515  {
1516  out << " '" << link_model->getName() << "' with " << link_model->getShapes().size() << " geoms\n";
1517  if (link_model->parentJointIsFixed())
1518  out << " * "
1519  << "parent joint is fixed" << '\n';
1520  if (link_model->jointOriginTransformIsIdentity())
1521  out << " * "
1522  << "joint origin transform is identity\n";
1523  }
1524 
1525  out << "Available groups: \n";
1526  for (JointModelGroup* joint_model_group : joint_model_groups_)
1527  joint_model_group->printGroupInfo(out);
1528 }
1529 
1530 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
1531  LinkTransformMap& associated_transforms)
1532 {
1533  associated_transforms[link] = transform * link->getJointOriginTransform();
1534  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1535  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1536  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1537  transform * link->getJointOriginTransform(), associated_transforms);
1538 }
1539 
1540 } // end of namespace core
1541 } // end of namespace moveit
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint,...
Definition: joint_model.h:162
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
Definition: joint_model.h:168
JointType getType() const
Get the type of joint.
Definition: joint_model.h:151
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:108
const Eigen::Isometry3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link's origin....
Definition: link_model.h:143
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
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
MotionModel
different types of planar joints we support
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:511
std::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:418
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:481
std::vector< const JointModel * > single_dof_joints_
Definition: robot_model.h:545
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:525
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:508
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:595
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:505
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:483
void computeFixedTransforms(const LinkModel *link, const Eigen::Isometry3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
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:150
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:499
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:502
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for the joints is stored.
Definition: robot_model.h:556
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:589
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
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:522
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:543
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:77
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
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
size_t getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:62
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:575
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:537
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:578
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:493
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
std::vector< std::string > active_joint_model_names_vector_
The vector of joint names that corresponds to active_joint_model_vector_.
Definition: robot_model.h:534
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:562
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:572
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:485
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:592
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:496
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:570
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
Definition: robot_model.h:583
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:598
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
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
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:547
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:540
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:586
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 printModelInfo(std::ostream &out) const
Print information about the constructed model.
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:528
double distance(const double *state1, const double *state2) const
Return the sum of joint distances between two states. An L1 norm. Only considers active joints.
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:213
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:519
std::size_t variable_count_
Get the number of variables necessary to describe this model.
Definition: robot_model.h:565
double getMaximumExtent() const
Definition: robot_model.h:350
std::string model_name_
The name of the robot.
Definition: robot_model.h:477
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:490
const LinkModel * getLinkModel(const std::string &link, bool *has_link=nullptr) const
Get a link by its name. Output error and return nullptr when the link is missing.
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:344
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:531
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return nullptr when the joint is missing.
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:516
const std::string & getName() const
Get the model name.
Definition: robot_model.h:85
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:339
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
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:68
std::vector< const JointModel::Bounds * > JointBoundsVector
Main namespace for MoveIt.
Definition: exceptions.h:43
a
Definition: plan.py:54
d
Definition: setup.py:4
name
Definition: setup.py:7
Definition: world.h:49