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 (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 
72 const JointModel* RobotModel::getRootJoint() const
73 {
74  return root_joint_;
75 }
76 
77 const LinkModel* RobotModel::getRootLink() const
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 (const JointModel* parent : parents)
143  descendants[parent].second.insert(joint);
144 
145  const LinkModel* lm = joint->getChildLinkModel();
146  if (!lm)
147  return;
148 
149  for (const JointModel* parent : parents)
150  descendants[parent].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 (const JointModel* child_joint_model : ch)
156  computeDescendantsHelper(child_joint_model, parents, seen, descendants);
157  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
158  for (const JointModel* mimic_joint_model : mim)
159  computeDescendantsHelper(mimic_joint_model, 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 (const JointModel* m : b)
179  common_roots[ch[i]->getJointIndex() * size + m->getJointIndex()] =
180  common_roots[ch[i]->getJointIndex() + m->getJointIndex() * size] = joint->getJointIndex();
181  for (const JointModel* k : a)
182  {
183  common_roots[k->getJointIndex() * size + ch[j]->getJointIndex()] =
184  common_roots[k->getJointIndex() + ch[j]->getJointIndex() * size] = joint->getJointIndex();
185  for (const JointModel* m : b)
186  common_roots[k->getJointIndex() * size + m->getJointIndex()] =
187  common_roots[k->getJointIndex() + 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 (const JointModel* joint_model : joint_model_vector_)
212  {
213  // the common root of a joint and itself is the same joint:
214  common_joint_roots_[joint_model->getJointIndex() * (1 + joint_model_vector_.size())] = joint_model->getJointIndex();
215 
216  // a node N and one of its descendants have as common root the node N itself:
217  const std::vector<const JointModel*>& d = joint_model->getDescendantJointModels();
218  for (const JointModel* descendant_joint_model : d)
219  common_joint_roots_[descendant_joint_model->getJointIndex() * joint_model_vector_.size() +
220  joint_model->getJointIndex()] =
221  common_joint_roots_[descendant_joint_model->getJointIndex() +
222  joint_model->getJointIndex() * joint_model_vector_.size()] = joint_model->getJointIndex();
223  }
224 }
225 
227 {
228  // compute the list of descendants for all joints
229  std::vector<const JointModel*> parents;
230  std::set<const JointModel*> seen;
231 
232  DescMap descendants;
233  computeDescendantsHelper(root_joint_, parents, seen, descendants);
234  for (std::pair<const JointModel* const, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
235  std::set<const JointModel*, OrderJointsByIndex>>>& descendant :
236  descendants)
237  {
238  JointModel* jm = const_cast<JointModel*>(descendant.first);
239  for (const JointModel* jt : descendant.second.second)
240  jm->addDescendantJointModel(jt);
241  for (const LinkModel* jt : descendant.second.first)
242  jm->addDescendantLinkModel(jt);
243  }
244 }
245 
247 {
249  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildJointInfo");
250 
251  // construct additional maps for easy access by name
252  variable_count_ = 0;
254  variable_names_.reserve(joint_model_vector_.size());
256 
257  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
258  {
259  joint_model_vector_[i]->setJointIndex(i);
260  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
261 
262  // compute index map
263  if (!name_order.empty())
264  {
265  for (std::size_t j = 0; j < name_order.size(); ++j)
266  {
267  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
268  variable_names_.push_back(name_order[j]);
270  }
271  if (joint_model_vector_[i]->getMimic() == nullptr)
272  {
277  }
278 
279  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
280  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
282 
283  joint_model_vector_[i]->setFirstVariableIndex(variable_count_);
285 
286  // compute variable count
287  std::size_t vc = joint_model_vector_[i]->getVariableCount();
288  variable_count_ += vc;
289  if (vc == 1)
291  else
293  }
294  }
295 
296  std::vector<bool> link_considered(link_model_vector_.size(), false);
297  for (const LinkModel* link : link_model_vector_)
298  {
299  if (link_considered[link->getLinkIndex()])
300  continue;
301 
302  LinkTransformMap associated_transforms;
303  computeFixedTransforms(link, link->getJointOriginTransform().inverse(), associated_transforms);
304  for (auto& tf_base : associated_transforms)
305  {
306  link_considered[tf_base.first->getLinkIndex()] = true;
307  for (auto& tf_target : associated_transforms)
308  {
309  if (&tf_base != &tf_target)
310  const_cast<LinkModel*>(tf_base.first) // regain write access to base LinkModel*
311  ->addAssociatedFixedTransform(tf_target.first, tf_base.second.inverse() * tf_target.second);
312  }
313  }
314  }
315 
317  computeCommonRoots(); // must be called _after_ list of descendants was computed
318 }
319 
320 void RobotModel::buildGroupStates(const srdf::Model& srdf_model)
321 {
322  // copy the default states to the groups
323  const std::vector<srdf::Model::GroupState>& ds = srdf_model.getGroupStates();
324  for (const srdf::Model::GroupState& group_state : ds)
325  {
326  if (hasJointModelGroup(group_state.group_))
327  {
328  JointModelGroup* jmg = getJointModelGroup(group_state.group_);
329  std::vector<const JointModel*> remaining_joints = jmg->getActiveJointModels();
330  std::map<std::string, double> state;
331  for (std::map<std::string, std::vector<double>>::const_iterator jt = group_state.joint_values_.begin();
332  jt != group_state.joint_values_.end(); ++jt)
333  {
334  if (jmg->hasJointModel(jt->first))
335  {
336  const JointModel* jm = jmg->getJointModel(jt->first);
337  const std::vector<std::string>& vn = jm->getVariableNames();
338  // Remove current joint name from remaining list.
339  auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm);
340  if (it_found != remaining_joints.end())
341  remaining_joints.erase(it_found);
342  if (vn.size() == jt->second.size())
343  for (std::size_t j = 0; j < vn.size(); ++j)
344  state[vn[j]] = jt->second[j];
345  else
347  "The model for joint '%s' requires %d variable values, "
348  "but only %d variable values were supplied in default state '%s' for group '%s'",
349  jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), group_state.name_.c_str(),
350  jmg->getName().c_str());
351  }
352  else
354  "Group state '%s' specifies value for joint '%s', "
355  "but that joint is not part of group '%s'",
356  group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
357  }
358  if (!remaining_joints.empty())
359  {
360  std::stringstream missing;
361  missing << (*remaining_joints.begin())->getName();
362  for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); j++)
363  {
364  missing << ", " << (*j)->getName();
365  }
366  ROS_WARN_STREAM_NAMED(LOGNAME, "Group state '" << group_state.name_
367  << "' doesn't specify all group joints in group '"
368  << group_state.group_ << "'. " << missing.str() << " "
369  << (remaining_joints.size() > 1 ? "are" : "is") << " missing.");
370  }
371  if (!state.empty())
372  jmg->addDefaultState(group_state.name_, state);
373  }
374  else
375  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specified for group '%s', but that group does not exist",
376  group_state.name_.c_str(), group_state.group_.c_str());
377  }
378 }
379 
380 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
381 {
382  // compute mimic joints
383  for (JointModel* joint_model : joint_model_vector_)
384  {
385  const urdf::Joint* jm = urdf_model.getJoint(joint_model->getName()).get();
386  if (jm)
387  if (jm->mimic)
388  {
389  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
390  if (jit != joint_model_map_.end())
391  {
392  if (joint_model->getVariableCount() == jit->second->getVariableCount())
393  joint_model->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
394  else
395  ROS_ERROR_NAMED(LOGNAME, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
396  joint_model->getName().c_str(), jm->mimic->joint_name.c_str());
397  }
398  else
399  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' cannot mimic unknown joint '%s'", joint_model->getName().c_str(),
400  jm->mimic->joint_name.c_str());
401  }
402  }
403 
404  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
405  bool change = true;
406  while (change)
407  {
408  change = false;
409  for (JointModel* joint_model : joint_model_vector_)
410  if (joint_model->getMimic())
411  {
412  if (joint_model->getMimic()->getMimic())
413  {
414  joint_model->setMimic(joint_model->getMimic()->getMimic(),
415  joint_model->getMimicFactor() * joint_model->getMimic()->getMimicFactor(),
416  joint_model->getMimicOffset() +
417  joint_model->getMimicFactor() * joint_model->getMimic()->getMimicOffset());
418  change = true;
419  }
420  if (joint_model == joint_model->getMimic())
421  {
422  ROS_ERROR_NAMED(LOGNAME, "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
423  for (JointModel* joint_model_recal : joint_model_vector_)
424  joint_model_recal->setMimic(nullptr, 0.0, 0.0);
425  change = false;
426  break;
427  }
428  }
429  }
430  // build mimic requests
431  for (JointModel* joint_model : joint_model_vector_)
432  if (joint_model->getMimic())
433  {
434  const_cast<JointModel*>(joint_model->getMimic())->addMimicRequest(joint_model);
435  mimic_joints_.push_back(joint_model);
436  }
437 }
438 
439 bool RobotModel::hasEndEffector(const std::string& eef) const
440 {
441  return end_effectors_map_.find(eef) != end_effectors_map_.end();
442 }
443 
444 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
445 {
446  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
447  if (it == end_effectors_map_.end())
448  {
449  it = joint_model_group_map_.find(name);
450  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
451  return it->second;
452  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
453  return nullptr;
454  }
455  return it->second;
456 }
457 
458 JointModelGroup* RobotModel::getEndEffector(const std::string& name)
459 {
460  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
461  if (it == end_effectors_map_.end())
462  {
463  it = joint_model_group_map_.find(name);
464  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
465  return it->second;
466  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
467  return nullptr;
468  }
469  return it->second;
470 }
471 
472 bool RobotModel::hasJointModelGroup(const std::string& name) const
473 {
474  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
475 }
476 
477 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
478 {
479  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
480  if (it == joint_model_group_map_.end())
481  {
482  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
483  return nullptr;
484  }
485  return it->second;
486 }
487 
488 JointModelGroup* RobotModel::getJointModelGroup(const std::string& name)
489 {
490  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
491  if (it == joint_model_group_map_.end())
492  {
493  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
494  return nullptr;
495  }
496  return it->second;
497 }
498 
499 void RobotModel::buildGroups(const srdf::Model& srdf_model)
500 {
501  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
502 
503  // the only thing tricky is dealing with subgroups
504  std::vector<bool> processed(group_configs.size(), false);
505 
506  bool added = true;
507  while (added)
508  {
509  added = false;
510 
511  // going to make passes until we can't do anything else
512  for (std::size_t i = 0; i < group_configs.size(); ++i)
513  if (!processed[i])
514  {
515  // if we haven't processed, check and see if the dependencies are met yet
516  bool all_subgroups_added = true;
517  for (const std::string& subgroup : group_configs[i].subgroups_)
518  if (joint_model_group_map_.find(subgroup) == joint_model_group_map_.end())
519  {
520  all_subgroups_added = false;
521  break;
522  }
523  if (all_subgroups_added)
524  {
525  added = true;
526  processed[i] = true;
527  if (!addJointModelGroup(group_configs[i]))
528  ROS_WARN_NAMED(LOGNAME, "Failed to add group '%s'", group_configs[i].name_.c_str());
529  }
530  }
531  }
532 
533  for (std::size_t i = 0; i < processed.size(); ++i)
534  if (!processed[i])
535  ROS_WARN_NAMED(LOGNAME, "Could not process group '%s' due to unmet subgroup dependencies",
536  group_configs[i].name_.c_str());
537 
538  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
539  joint_model_groups_.push_back(it->second);
540  std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
541  for (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(srdf_model);
548  buildGroupsInfoEndEffectors(srdf_model);
549 }
550 
552 {
553  // compute subgroups
554  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
555  {
556  JointModelGroup* jmg = it->second;
557  std::vector<std::string> subgroup_names;
558  std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
559  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
560  ++jt)
561  if (jt->first != it->first)
562  {
563  bool ok = true;
564  JointModelGroup* sub_jmg = jt->second;
565  const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
566  for (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 
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  ROS_ERROR_NAMED(LOGNAME, "Group '%s' for end-effector '%s' cannot be its own parent",
622  eef.parent_group_.c_str(), eef.name_.c_str());
623  }
624  else
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  ROS_ERROR_NAMED(LOGNAME, "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  ROS_WARN_NAMED(LOGNAME, "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 
664 {
666  {
667  ROS_WARN_NAMED(LOGNAME, "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());
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  ROS_WARN_NAMED(LOGNAME, "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 = urdf_link->parent_joint ?
783  constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
784  constructJointModel(nullptr, urdf_link, srdf_model);
785  if (joint == nullptr)
786  return nullptr;
787 
788  // bookkeeping for the joint
789  joint_model_map_[joint->getName()] = joint;
790  joint->setJointIndex(joint_model_vector_.size());
791  joint_model_vector_.push_back(joint);
792  joint_model_vector_const_.push_back(joint);
793  joint_model_names_vector_.push_back(joint->getName());
794  joint->setParentLinkModel(parent);
795 
796  // construct the link
797  LinkModel* link = constructLinkModel(urdf_link);
798  joint->setChildLinkModel(link);
799  link->setParentLinkModel(parent);
800 
801  // bookkeeping for the link
802  link_model_map_[joint->getChildLinkModel()->getName()] = link;
803  link->setLinkIndex(link_model_vector_.size());
804  link_model_vector_.push_back(link);
805  link_model_vector_const_.push_back(link);
806  link_model_names_vector_.push_back(link->getName());
807  if (!link->getShapes().empty())
808  {
810  link_model_names_with_collision_geometry_vector_.push_back(link->getName());
811  link->setFirstCollisionBodyTransformIndex(link_geometry_count_);
812  link_geometry_count_ += link->getShapes().size();
813  }
814  link->setParentJointModel(joint);
815 
816  // recursively build child links (and joints)
817  for (const urdf::LinkSharedPtr& child_link : urdf_link->child_links)
818  {
819  JointModel* jm = buildRecursive(link, child_link.get(), srdf_model);
820  if (jm)
821  link->addChildJointModel(jm);
822  }
823  return joint;
824 }
825 
826 namespace
827 {
828 // construct bounds for 1DOF joint
829 static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
830 {
831  VariableBounds b;
832  if (urdf_joint->safety)
833  {
834  b.position_bounded_ = true;
835  b.min_position_ = urdf_joint->safety->soft_lower_limit;
836  b.max_position_ = urdf_joint->safety->soft_upper_limit;
837  if (urdf_joint->limits)
838  {
839  if (urdf_joint->limits->lower > b.min_position_)
840  b.min_position_ = urdf_joint->limits->lower;
841  if (urdf_joint->limits->upper < b.max_position_)
842  b.max_position_ = urdf_joint->limits->upper;
843  }
844  }
845  else
846  {
847  if (urdf_joint->limits)
848  {
849  b.position_bounded_ = true;
850  b.min_position_ = urdf_joint->limits->lower;
851  b.max_position_ = urdf_joint->limits->upper;
852  }
853  }
854  if (urdf_joint->limits)
855  {
856  b.max_velocity_ = fabs(urdf_joint->limits->velocity);
857  b.min_velocity_ = -b.max_velocity_;
858  b.velocity_bounded_ = b.max_velocity_ > std::numeric_limits<double>::epsilon();
859  }
860  return b;
861 }
862 } // namespace
863 
864 JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link,
865  const srdf::Model& srdf_model)
866 {
867  JointModel* new_joint_model = nullptr;
868 
869  // if urdf_joint exists, must be the root link transform
870  if (urdf_joint)
871  {
872  switch (urdf_joint->type)
873  {
874  case urdf::Joint::REVOLUTE:
875  {
876  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
877  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
878  j->setContinuous(false);
879  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
880  new_joint_model = j;
881  }
882  break;
883  case urdf::Joint::CONTINUOUS:
884  {
885  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
886  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
887  j->setContinuous(true);
888  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
889  new_joint_model = j;
890  }
891  break;
892  case urdf::Joint::PRISMATIC:
893  {
894  PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name);
895  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
896  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
897  new_joint_model = j;
898  }
899  break;
900  case urdf::Joint::FLOATING:
901  new_joint_model = new FloatingJointModel(urdf_joint->name);
902  break;
903  case urdf::Joint::PLANAR:
904  new_joint_model = new PlanarJointModel(urdf_joint->name);
905  break;
906  case urdf::Joint::FIXED:
907  new_joint_model = new FixedJointModel(urdf_joint->name);
908  break;
909  default:
910  ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type);
911  break;
912  }
913  }
914  else // if urdf_joint passed in as null, then we're at root of URDF model
915  {
916  const std::vector<srdf::Model::VirtualJoint>& virtual_joints = srdf_model.getVirtualJoints();
917  for (const srdf::Model::VirtualJoint& virtual_joint : virtual_joints)
918  {
919  if (virtual_joint.child_link_ != child_link->name)
920  {
922  "Skipping virtual joint '%s' because its child frame '%s' "
923  "does not match the URDF frame '%s'",
924  virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str());
925  }
926  else if (virtual_joint.parent_frame_.empty())
927  {
928  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its parent frame is empty",
929  virtual_joint.name_.c_str());
930  }
931  else
932  {
933  if (virtual_joint.type_ == "fixed")
934  new_joint_model = new FixedJointModel(virtual_joint.name_);
935  else if (virtual_joint.type_ == "planar")
936  new_joint_model = new PlanarJointModel(virtual_joint.name_);
937  else if (virtual_joint.type_ == "floating")
938  new_joint_model = new FloatingJointModel(virtual_joint.name_);
939  if (new_joint_model)
940  {
941  // for fixed frames we still use the robot root link
942  if (virtual_joint.type_ != "fixed")
943  {
944  model_frame_ = virtual_joint.parent_frame_;
945  }
946  break;
947  }
948  }
949  }
950  if (!new_joint_model)
951  {
952  ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint");
953  new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
954  }
955  }
956 
957  if (new_joint_model)
958  {
959  new_joint_model->setDistanceFactor(new_joint_model->getStateSpaceDimension());
960  const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
961  for (const srdf::Model::PassiveJoint& pjoint : pjoints)
962  {
963  if (new_joint_model->getName() == pjoint.name_)
964  {
965  new_joint_model->setPassive(true);
966  break;
967  }
968  }
969  }
970 
971  return new_joint_model;
972 }
973 
974 namespace
975 {
976 static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose)
977 {
978  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
979  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
980  return af;
981 }
982 } // namespace
983 
984 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
985 {
986  LinkModel* new_link_model = new LinkModel(urdf_link->name);
987 
988  const std::vector<urdf::CollisionSharedPtr>& col_array =
989  urdf_link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, urdf_link->collision) :
990  urdf_link->collision_array;
991 
992  std::vector<shapes::ShapeConstPtr> shapes;
993  EigenSTL::vector_Isometry3d poses;
994 
995  for (const urdf::CollisionSharedPtr& col : col_array)
996  {
997  if (col && col->geometry)
998  {
999  shapes::ShapeConstPtr s = constructShape(col->geometry.get());
1000  if (s)
1001  {
1002  shapes.push_back(s);
1003  poses.push_back(urdfPose2Isometry3d(col->origin));
1004  }
1005  }
1006  }
1007 
1008  // Should we warn that old (melodic) behaviour has changed, not copying visual to collision geometries anymore?
1009  bool warn_about_missing_collision = false;
1010  if (shapes.empty())
1011  {
1012  const auto& vis_array = urdf_link->visual_array.empty() ? std::vector<urdf::VisualSharedPtr>{ urdf_link->visual } :
1013  urdf_link->visual_array;
1014  for (const urdf::VisualSharedPtr& vis : vis_array)
1015  {
1016  if (vis && vis->geometry)
1017  warn_about_missing_collision = true;
1018  }
1019  }
1020  if (warn_about_missing_collision)
1021  {
1022  ROS_WARN_STREAM_NAMED(LOGNAME + ".empty_collision_geometry",
1023  "Link " << urdf_link->name
1024  << " has visual geometry but no collision geometry. "
1025  "Collision geometry will be left empty. "
1026  "Fix your URDF file by explicitly specifying collision geometry.");
1027  }
1028 
1029  new_link_model->setGeometry(shapes, poses);
1030 
1031  // figure out visual mesh (try visual urdf tag first, collision tag otherwise
1032  if (urdf_link->visual && urdf_link->visual->geometry)
1033  {
1034  if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
1035  {
1036  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
1037  if (!mesh->filename.empty())
1038  new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin),
1039  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1040  }
1041  }
1042  else if (urdf_link->collision && urdf_link->collision->geometry)
1043  {
1044  if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
1045  {
1046  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
1047  if (!mesh->filename.empty())
1048  new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin),
1049  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1050  }
1051  }
1052 
1053  if (urdf_link->parent_joint)
1054  new_link_model->setJointOriginTransform(
1055  urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
1056 
1057  return new_link_model;
1058 }
1059 
1060 shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom)
1061 {
1062  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
1063 
1064  shapes::Shape* new_shape = nullptr;
1065  switch (geom->type)
1066  {
1067  case urdf::Geometry::SPHERE:
1068  new_shape = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
1069  break;
1070  case urdf::Geometry::BOX:
1071  {
1072  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
1073  new_shape = new shapes::Box(dim.x, dim.y, dim.z);
1074  }
1075  break;
1076  case urdf::Geometry::CYLINDER:
1077  new_shape = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
1078  static_cast<const urdf::Cylinder*>(geom)->length);
1079  break;
1080  case urdf::Geometry::MESH:
1081  {
1082  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
1083  if (!mesh->filename.empty())
1084  {
1085  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
1086  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
1087  new_shape = m;
1088  }
1089  }
1090  break;
1091  default:
1092  ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type);
1093  break;
1094  }
1095 
1096  return shapes::ShapePtr(new_shape);
1097 }
1098 
1099 bool RobotModel::hasJointModel(const std::string& name) const
1100 {
1101  return joint_model_map_.find(name) != joint_model_map_.end();
1102 }
1103 
1104 bool RobotModel::hasLinkModel(const std::string& name) const
1105 {
1106  return link_model_map_.find(name) != link_model_map_.end();
1107 }
1108 
1109 const JointModel* RobotModel::getJointModel(const std::string& name) const
1110 {
1111  JointModelMap::const_iterator it = joint_model_map_.find(name);
1112  if (it != joint_model_map_.end())
1113  return it->second;
1114  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1115  return nullptr;
1116 }
1117 
1118 const JointModel* RobotModel::getJointModel(int index) const
1119 {
1120  if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
1121  {
1122  ROS_ERROR_NAMED(LOGNAME, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
1123  return nullptr;
1124  }
1125  assert(joint_model_vector_[index]->getJointIndex() == index);
1127 }
1128 
1129 JointModel* RobotModel::getJointModel(const std::string& name)
1130 {
1131  JointModelMap::const_iterator it = joint_model_map_.find(name);
1132  if (it != joint_model_map_.end())
1133  return it->second;
1134  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1135  return nullptr;
1136 }
1137 
1138 const LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) const
1139 {
1140  return const_cast<RobotModel*>(this)->getLinkModel(name, has_link);
1141 }
1142 
1143 const LinkModel* RobotModel::getLinkModel(int index) const
1144 {
1145  if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
1146  {
1147  ROS_ERROR_NAMED(LOGNAME, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
1148  return nullptr;
1149  }
1150  assert(link_model_vector_[index]->getLinkIndex() == index);
1151  return link_model_vector_[index];
1152 }
1153 
1154 LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
1155 {
1156  if (has_link)
1157  *has_link = true; // Start out optimistic
1158  LinkModelMap::const_iterator it = link_model_map_.find(name);
1159  if (it != link_model_map_.end())
1160  return it->second;
1161 
1162  if (has_link)
1163  *has_link = false; // Report failure via argument
1164  else // Otherwise print error
1165  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1166  return nullptr;
1167 }
1168 
1171  if (!link)
1172  return link;
1173  const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
1174  const moveit::core::JointModel* joint = link->getParentJointModel();
1176  while (parent_link && joint->getType() == moveit::core::JointModel::FIXED)
1177  {
1178  link = parent_link;
1179  joint = link->getParentJointModel();
1180  parent_link = joint->getParentLinkModel();
1181  }
1182  return link;
1183 }
1185 void RobotModel::updateMimicJoints(double* values) const
1186 {
1187  for (const JointModel* mimic_joint : mimic_joints_)
1188  {
1189  int src = mimic_joint->getMimic()->getFirstVariableIndex();
1190  int dest = mimic_joint->getFirstVariableIndex();
1191  values[dest] = values[src] * mimic_joint->getMimicFactor() + mimic_joint->getMimicOffset();
1192  }
1193 }
1194 
1196 {
1197  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1200 }
1201 
1203  std::map<std::string, double>& values) const
1205  std::vector<double> tmp(variable_count_);
1206  getVariableRandomPositions(rng, &tmp[0]);
1207  values.clear();
1208  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1209  values[variable_names_[i]] = tmp[i];
1210 }
1211 
1212 void RobotModel::getVariableDefaultPositions(double* values) const
1213 {
1214  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1217 }
1218 
1219 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1221  std::vector<double> tmp(variable_count_);
1222  getVariableDefaultPositions(&tmp[0]);
1223  values.clear();
1224  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1225  values[variable_names_[i]] = tmp[i];
1226 }
1227 
1228 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1229  std::vector<std::string>& missing_variables) const
1230 {
1231  missing_variables.clear();
1232  std::set<std::string> keys(variables.begin(), variables.end());
1233  for (const std::string& variable_name : variable_names_)
1234  if (keys.find(variable_name) == keys.end())
1235  if (getJointOfVariable(variable_name)->getMimic() == nullptr)
1236  missing_variables.push_back(variable_name);
1237 }
1238 
1239 int RobotModel::getVariableIndex(const std::string& variable) const
1240 {
1241  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1242  if (it == joint_variables_index_map_.end())
1243  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1244  return it->second;
1245 }
1246 
1247 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1248 {
1249  double max_distance = 0.0;
1250  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1251  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1252  active_joint_model_vector_[j]->getDistanceFactor();
1253  return max_distance;
1254 }
1255 
1256 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1257  double margin) const
1258 {
1259  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1260  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1262  *active_joint_bounds[i], margin))
1263  return false;
1264  return true;
1265 }
1266 
1267 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1269  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1270  bool change = false;
1271  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1273  *active_joint_bounds[i]))
1274  change = true;
1275  if (change)
1276  updateMimicJoints(state);
1277  return change;
1279 
1280 double RobotModel::distance(const double* state1, const double* state2) const
1281 {
1282  double d = 0.0;
1283  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1284  d += active_joint_model_vector_[i]->getDistanceFactor() *
1286  state2 + active_joint_model_start_index_[i]);
1287  return d;
1288 }
1289 
1290 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1291 {
1292  // we interpolate values only for active joint models (non-mimic)
1293  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1296  state + active_joint_model_start_index_[i]);
1297  // now we update mimic as needed
1298  updateMimicJoints(state);
1299 }
1300 
1301 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1302 {
1303  // we first set all the "simple" allocators -- where a group has one IK solver
1305  {
1306  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1307  if (jt != allocators.end())
1308  {
1309  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> solver_allocator_pair;
1310  solver_allocator_pair.first = jt->second;
1311  jmg->setSolverAllocators(solver_allocator_pair);
1312  }
1313  }
1314 
1315  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1316  // setSolverAllocators()
1318  {
1319  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> solver_allocator_pair;
1320  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1321  if (jt == allocators.end())
1322  {
1323  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1324  std::set<const JointModel*> joints;
1325  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1326 
1327  std::vector<const JointModelGroup*> subs;
1328 
1329  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1330  for (const std::pair<const std::string, SolverAllocatorFn>& allocator : allocators)
1331  {
1332  const JointModelGroup* sub = getJointModelGroup(allocator.first);
1333  if (!sub) // this should actually not happen, all groups should be well defined
1334  {
1335  subs.clear();
1336  break;
1337  }
1338  std::set<const JointModel*> sub_joints;
1339  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1340 
1341  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1342  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1343  std::set<const JointModel*> joint_model_set;
1344  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1345  std::inserter(joint_model_set, joint_model_set.end()));
1346  // TODO: instead of maintaining disjoint joint sets here,
1347  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1348  // There, a disjoint bijection from joints to solvers is computed anyway.
1349  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1350  // But, if the overlap only involves fixed joints, we could consider all sub groups
1351  subs.push_back(sub);
1352  joints.swap(joint_model_set);
1353  }
1354  }
1355 
1356  // if we found subgroups, pass that information to the planning group
1357  if (!subs.empty())
1358  {
1359  std::stringstream ss;
1360  for (const JointModelGroup* sub : subs)
1361  {
1362  ss << sub->getName() << " ";
1363  solver_allocator_pair.second[sub] = allocators.find(sub->getName())->second;
1364  }
1365  ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1366  ss.str().c_str());
1367  }
1368  jmg->setSolverAllocators(solver_allocator_pair);
1369  }
1370  }
1371 }
1372 
1373 void RobotModel::printModelInfo(std::ostream& out) const
1374 {
1375  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
1376  << std::endl;
1377 
1378  std::ios_base::fmtflags old_flags = out.flags();
1379  out.setf(std::ios::fixed, std::ios::floatfield);
1380  std::streamsize old_prec = out.precision();
1381  out.precision(5);
1382  out << "Joints: " << std::endl;
1383  for (JointModel* joint_model : joint_model_vector_)
1384  {
1385  out << " '" << joint_model->getName() << "' (" << joint_model->getTypeName() << ")" << std::endl;
1386  out << " * Joint Index: " << joint_model->getJointIndex() << std::endl;
1387  const std::vector<std::string>& vn = joint_model->getVariableNames();
1388  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
1389  << std::endl;
1390  int idx = joint_model->getFirstVariableIndex();
1391  for (const std::string& it : vn)
1392  {
1393  out << " * '" << it << "', index " << idx++ << " in full state";
1394  if (joint_model->getMimic())
1395  out << ", mimic '" << joint_model->getMimic()->getName() << "'";
1396  if (joint_model->isPassive())
1397  out << ", passive";
1398  out << std::endl;
1399  out << " " << joint_model->getVariableBounds(it) << std::endl;
1400  }
1401  }
1402  out << std::endl;
1403  out.precision(old_prec);
1404  out.flags(old_flags);
1405  out << "Links: " << std::endl;
1406  for (LinkModel* link_model : link_model_vector_)
1407  {
1408  out << " '" << link_model->getName() << "' with " << link_model->getShapes().size() << " geoms" << std::endl;
1409  if (link_model->parentJointIsFixed())
1410  out << " * "
1411  << "parent joint is fixed" << std::endl;
1412  if (link_model->jointOriginTransformIsIdentity())
1413  out << " * "
1414  << "joint origin transform is identity" << std::endl;
1415  }
1416 
1417  out << "Available groups: " << std::endl;
1418  for (JointModelGroup* joint_model_group : joint_model_groups_)
1419  joint_model_group->printGroupInfo(out);
1420 }
1421 
1422 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
1423  LinkTransformMap& associated_transforms)
1424 {
1425  associated_transforms[link] = transform * link->getJointOriginTransform();
1426  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1427  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1428  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1429  transform * link->getJointOriginTransform(), associated_transforms);
1430 }
1431 
1432 } // end of namespace core
1433 } // end of namespace moveit
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core::RobotModel::variable_count_
std::size_t variable_count_
Get the number of variables necessary to describe this model.
Definition: robot_model.h:603
moveit::core::RobotModel::getJointModel
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return NULL when the joint is missing.
Definition: robot_model.cpp:1175
moveit::core::RobotModel::end_effectors_
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:636
shapes
moveit::core::FixedJointModel
A fixed joint.
Definition: fixed_joint_model.h:110
moveit::core::RobotModel::active_joint_model_vector_
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:572
name_
std::string name_
moveit::core::RobotModel::active_joint_model_vector_const_
std::vector< const JointModel * > active_joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:575
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::LinkModel::getChildJointModels
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:127
moveit::core::RobotModel::setKinematicsAllocators
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
Definition: robot_model.cpp:1367
moveit::core::RobotModel::constructLinkModel
LinkModel * constructLinkModel(const urdf::Link *urdf_link)
Given a urdf link, build the corresponding LinkModel object.
Definition: robot_model.cpp:1050
moveit::core::RobotModel::joint_model_vector_const_
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:566
moveit::core::PlanarJointModel
A planar joint.
Definition: planar_joint_model.h:110
moveit::core::RobotModel::addJointModelGroup
bool addJointModelGroup(const srdf::Model::Group &group)
Construct a JointModelGroup given a SRDF description group.
Definition: robot_model.cpp:729
s
XmlRpcServer s
moveit::core::RobotModel::joint_model_groups_const_
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:630
srdf::Model::Group::chains_
std::vector< std::pair< std::string, std::string > > chains_
srdf::Model::getPassiveJoints
const std::vector< PassiveJoint > & getPassiveJoints() const
moveit::core::RobotModel::link_geometry_count_
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:552
moveit::core::RobotModel::computeDescendants
void computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
Definition: robot_model.cpp:292
moveit::core::RobotModel::joint_model_names_vector_
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:569
moveit::core::RobotModel::getVariableBounds
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:474
moveit::core::RobotModel::continuous_joint_model_vector_
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:578
moveit::core::RobotModel::getRootJoint
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:138
moveit::core::RobotModel::joint_model_vector_
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:563
shapes::Cylinder
moveit::core::LinkModel::getParentJointModel
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:107
shape_operations.h
moveit::core::RobotModel::joint_model_group_map_
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
Definition: robot_model.h:621
moveit::core::JointBoundsVector
std::vector< const JointModel::Bounds * > JointBoundsVector
Definition: joint_model_group.h:132
moveit::tools::Profiler::ScopedStart
This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of s...
Definition: profiler.h:98
moveit::core::RobotModel::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:459
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotModel::buildRecursive
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),...
Definition: robot_model.cpp:845
shapes::Shape
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
shapes::Mesh
moveit::core::RobotModel::computeCommonRoots
void computeCommonRoots()
For every pair of joints, pre-compute the common roots of the joints.
Definition: robot_model.cpp:261
moveit::core::RobotModel::joint_model_group_names_
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:633
moveit::core::RobotModel::joint_model_groups_
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:627
moveit::core::RobotModel::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
Definition: robot_model.cpp:543
moveit::core::RobotModel::buildMimic
void buildMimic(const urdf::ModelInterface &urdf_model)
Given the URDF model, build up the mimic joints (mutually constrained joints)
Definition: robot_model.cpp:446
moveit::core::RobotModel::interpolate
void interpolate(const double *from, const double *to, double t, double *state) const
Definition: robot_model.cpp:1356
ok
ROSCPP_DECL bool ok()
robot_model.h
moveit::core::RobotModel::buildGroupsInfoSubgroups
void buildGroupsInfoSubgroups(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
Definition: robot_model.cpp:617
moveit::core::RobotModel::joint_model_map_
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:560
moveit::core::JointModel::getType
JointType getType() const
Get the type of joint.
Definition: joint_model.h:202
moveit::core::RobotModel::single_dof_joints_
std::vector< const JointModel * > single_dof_joints_
Definition: robot_model.h:583
moveit::core::JointModel::REVOLUTE
@ REVOLUTE
Definition: joint_model.h:180
moveit::core::RobotModel::link_model_map_
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:534
moveit::core::JointModel::getChildLinkModel
const LinkModel * getChildLinkModel() const
Get the link that this joint connects to. There will always be such a link.
Definition: joint_model.h:219
moveit::core::RobotModel::link_model_vector_const_
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:540
moveit::core::RobotModel::srdf_
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:524
moveit::core::RobotModel::link_model_vector_
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:537
moveit::core::JointModelGroup::getName
const std::string & getName() const
Get the name of the joint group.
Definition: joint_model_group.h:185
moveit::core::RobotModel::getMaximumExtent
double getMaximumExtent() const
Definition: robot_model.h:401
moveit::core::RobotModel::hasEndEffector
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
Definition: robot_model.cpp:505
profiler.h
moveit::core::JointModel::getParentLinkModel
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:213
srdf::Model::getGroupStates
const std::vector< GroupState > & getGroupStates() const
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
moveit::core::RobotModel::link_models_with_collision_geometry_vector_
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:546
moveit::core::RobotModel::common_joint_roots_
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:594
srdf::Model::Group::name_
std::string name_
moveit::core::RobotModel::mimic_joints_
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:581
shapes::Box
name
std::string name
moveit::core::RobotModel::model_frame_
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:522
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
shapes::Sphere
moveit::core::RobotModel::multi_dof_joints_
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:585
moveit::core::RobotModel::root_link_
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:531
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
moveit::core::RobotModel::link_model_names_vector_
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:543
moveit::core::RobotModel::urdf_
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:526
moveit::core::RobotModel::RobotModel
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:120
moveit::core::JointModel::getMimicRequests
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:456
moveit::core::FloatingJointModel
A floating joint.
Definition: floating_joint_model.h:110
moveit::core::RobotModel::getJointOfVariable
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:264
moveit::core::RobotModel::buildGroupsInfoEndEffectors
void buildGroupsInfoEndEffectors(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
Definition: robot_model.cpp:646
srdf::ModelConstSharedPtr
std::shared_ptr< const Model > ModelConstSharedPtr
srdf::Model::Group::links_
std::vector< std::string > links_
moveit::core::RobotModel::computeFixedTransforms
void computeFixedTransforms(const LinkModel *link, const Eigen::Isometry3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
Definition: robot_model.cpp:1488
moveit::core::RobotModel::hasJointModelGroup
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
Definition: robot_model.cpp:538
moveit::core::RobotModel::joints_of_variable_
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:616
moveit::core::RobotModel::satisfiesPositionBounds
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:395
d
d
moveit::core::RobotModel::active_joint_models_bounds_
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:613
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
moveit::core::RobotModel::enforcePositionBounds
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:390
srdf::Model::getGroups
const std::vector< Group > & getGroups() const
moveit::core::RobotModel::constructJointModel
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...
Definition: robot_model.cpp:930
moveit::core::JointModel::getJointIndex
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:279
moveit::core::RobotModel::link_model_names_with_collision_geometry_vector_
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:549
moveit::core::RobotModel::variable_names_
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:600
moveit::core::LOGNAME
const std::string LOGNAME
Definition: joint_model_group.cpp:165
moveit::core::RobotModel::buildGroupStates
void buildGroupStates(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build the default states defined in the SRDF.
Definition: robot_model.cpp:386
moveit::core::RobotModel::getRootLink
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:143
moveit::core::RobotModel::buildJointInfo
void buildJointInfo()
Compute helpful information about joints.
Definition: robot_model.cpp:312
moveit::core::LinkTransformMap
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
moveit::core::RobotModel::active_joint_model_start_index_
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:610
moveit::core::JointModelGroup::getJointModels
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Definition: joint_model_group.h:209
moveit::core::RobotModel::getName
const std::string & getName() const
Get the model name.
Definition: robot_model.h:142
srdf::Model::PassiveJoint
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
random_numbers::RandomNumberGenerator
srdf::Model::GroupState
moveit::core::RobotModel::printModelInfo
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
Definition: robot_model.cpp:1439
moveit::core::JointModelGroup::setSolverAllocators
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
Definition: joint_model_group.h:587
moveit::core::RobotModel::getLinkModel
const LinkModel * getLinkModel(const std::string &link, bool *has_link=nullptr) const
Get a link by its name. Output error and return NULL when the link is missing.
Definition: robot_model.cpp:1204
srdf::Model::VirtualJoint
moveit::core::JointModelGroup::setSubgroupNames
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
Definition: joint_model_group.cpp:332
moveit::core::RobotModel::getMissingVariableNames
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
Definition: robot_model.cpp:1294
moveit::core::RobotModel::getRigidlyConnectedParentLinkModel
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints.
Definition: robot_model.cpp:1235
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
moveit::core::RobotModel::model_name_
std::string model_name_
The name of the robot.
Definition: robot_model.h:518
moveit::core::RobotModel::getVariableIndex
int getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
Definition: robot_model.cpp:1305
moveit::tools::Profiler::ScopedBlock
This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of sc...
Definition: profiler.h:77
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
index
unsigned int index
moveit::core::RobotModel::distance
double distance(const double *state1, const double *state2) const
Definition: robot_model.cpp:1346
srdf::Model::getEndEffectors
const std::vector< EndEffector > & getEndEffectors() const
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
moveit::core::RobotModel::getJointModels
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:207
moveit::core::RevoluteJointModel
A revolute joint.
Definition: revolute_joint_model.h:110
moveit::core::RobotModel::hasJointModel
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
Definition: robot_model.cpp:1165
srdf::Model::Group::subgroups_
std::vector< std::string > subgroups_
moveit::core::RobotModel::constructShape
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...
Definition: robot_model.cpp:1126
moveit::core::RobotModel::joint_variables_index_map_
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:608
srdf::Model::Group
srdf::Model
moveit::Exception
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:84
moveit::core::RobotModel::root_joint_
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:557
moveit::core::RobotModel::buildGroups
void buildGroups(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build up the groups in this kinematic model.
Definition: robot_model.cpp:565
moveit::core::RobotModel::getVariableDefaultPositions
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
Definition: robot_model.cpp:1278
moveit::core::JointModel::FIXED
@ FIXED
Definition: joint_model.h:184
moveit::core::RobotModel::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
Definition: robot_model.cpp:1261
moveit::core::RobotModel::updateMimicJoints
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
Definition: robot_model.cpp:1251
moveit::core::RobotModel::~RobotModel
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:128
srdf::Model::Group::joints_
std::vector< std::string > joints_
moveit::core::RobotModel::buildModel
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:148
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
srdf::Model::getVirtualJoints
const std::vector< VirtualJoint > & getVirtualJoints() const
srdf::Model::EndEffector
moveit::core::RobotModel::hasLinkModel
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
Definition: robot_model.cpp:1170
moveit::core::RobotModel::getEndEffector
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
Definition: robot_model.cpp:510
moveit::core::LinkModel::getParentLinkModel
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:116
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::RobotModel::end_effectors_map_
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:624


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Nov 24 2020 03:26:40