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 namespace
53 {
54 constexpr char LOGNAME[] = "robot_model";
55 } // namespace
56 
57 RobotModel::RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model)
58 {
59  root_joint_ = nullptr;
60  urdf_ = urdf_model;
61  srdf_ = srdf_model;
62  buildModel(*urdf_model, *srdf_model);
63 }
64 
66 {
67  for (std::pair<const std::string, JointModelGroup*>& it : joint_model_group_map_)
68  delete it.second;
69  for (JointModel* joint_model : joint_model_vector_)
70  delete joint_model;
71  for (LinkModel* link_model : link_model_vector_)
72  delete link_model;
73 }
74 
75 const JointModel* RobotModel::getRootJoint() const
76 {
77  return root_joint_;
78 }
79 
80 const LinkModel* RobotModel::getRootLink() const
81 {
82  return root_link_;
83 }
84 
85 void RobotModel::buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model)
86 {
88  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildModel");
89 
90  root_joint_ = nullptr;
91  root_link_ = nullptr;
93  variable_count_ = 0;
94  model_name_ = urdf_model.getName();
95  ROS_INFO_NAMED(LOGNAME, "Loading robot model '%s'...", model_name_.c_str());
96 
97  if (urdf_model.getRoot())
98  {
99  const urdf::Link* root_link_ptr = urdf_model.getRoot().get();
100  model_frame_ = root_link_ptr->name;
101 
102  ROS_DEBUG_NAMED(LOGNAME, "... building kinematic chain");
103  root_joint_ = buildRecursive(nullptr, root_link_ptr, srdf_model);
104  if (root_joint_)
106  ROS_DEBUG_NAMED(LOGNAME, "... building mimic joints");
107  buildMimic(urdf_model);
108 
109  ROS_DEBUG_NAMED(LOGNAME, "... computing joint indexing");
110  buildJointInfo();
111 
113  ROS_WARN_NAMED(LOGNAME, "No geometry is associated to any robot links");
114 
115  // build groups
116 
117  ROS_DEBUG_NAMED(LOGNAME, "... constructing joint groups");
118  buildGroups(srdf_model);
119 
120  ROS_DEBUG_NAMED(LOGNAME, "... constructing joint group states");
121  buildGroupStates(srdf_model);
122 
123  // For debugging entire model
124  // printModelInfo(std::cout);
125  }
126  else
127  ROS_WARN_NAMED(LOGNAME, "No root link found");
128 }
129 
130 namespace
131 {
132 typedef std::map<const JointModel*, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
133  std::set<const JointModel*, OrderJointsByIndex>>>
134  DescMap;
135 
136 void computeDescendantsHelper(const JointModel* joint, std::vector<const JointModel*>& parents,
137  std::set<const JointModel*>& seen, DescMap& descendants)
138 {
139  if (!joint)
140  return;
141  if (seen.find(joint) != seen.end())
142  return;
143  seen.insert(joint);
144 
145  for (const JointModel* parent : parents)
146  descendants[parent].second.insert(joint);
147 
148  const LinkModel* lm = joint->getChildLinkModel();
149  if (!lm)
150  return;
151 
152  for (const JointModel* parent : parents)
153  descendants[parent].first.insert(lm);
154  descendants[joint].first.insert(lm);
155 
156  parents.push_back(joint);
157  const std::vector<const JointModel*>& ch = lm->getChildJointModels();
158  for (const JointModel* child_joint_model : ch)
159  computeDescendantsHelper(child_joint_model, parents, seen, descendants);
160  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
161  for (const JointModel* mimic_joint_model : mim)
162  computeDescendantsHelper(mimic_joint_model, parents, seen, descendants);
163  parents.pop_back();
164 }
165 
166 void computeCommonRootsHelper(const JointModel* joint, std::vector<int>& common_roots, int size)
167 {
168  if (!joint)
169  return;
170  const LinkModel* lm = joint->getChildLinkModel();
171  if (!lm)
172  return;
173 
174  const std::vector<const JointModel*>& ch = lm->getChildJointModels();
175  for (std::size_t i = 0; i < ch.size(); ++i)
176  {
177  const std::vector<const JointModel*>& a = ch[i]->getDescendantJointModels();
178  for (std::size_t j = i + 1; j < ch.size(); ++j)
179  {
180  const std::vector<const JointModel*>& b = ch[j]->getDescendantJointModels();
181  for (const JointModel* m : b)
182  common_roots[ch[i]->getJointIndex() * size + m->getJointIndex()] =
183  common_roots[ch[i]->getJointIndex() + m->getJointIndex() * size] = joint->getJointIndex();
184  for (const JointModel* k : a)
185  {
186  common_roots[k->getJointIndex() * size + ch[j]->getJointIndex()] =
187  common_roots[k->getJointIndex() + ch[j]->getJointIndex() * size] = joint->getJointIndex();
188  for (const JointModel* m : b)
189  common_roots[k->getJointIndex() * size + m->getJointIndex()] =
190  common_roots[k->getJointIndex() + m->getJointIndex() * size] = joint->getJointIndex();
191  }
192  }
193  computeCommonRootsHelper(ch[i], common_roots, size);
194  }
195 }
196 } // namespace
197 
199 {
200  // compute common roots for all pairs of joints;
201  // there are 3 cases of pairs (X, Y):
202  // X != Y && X and Y are not descendants of one another
203  // X == Y
204  // X != Y && X and Y are descendants of one another
205 
206  // by default, the common root is always the global root;
208 
209  // look at all descendants recursively; for two sibling nodes A, B, both children of X, all the pairs of respective
210  // descendants of A and B
211  // have X as the common root.
212  computeCommonRootsHelper(root_joint_, common_joint_roots_, joint_model_vector_.size());
213 
214  for (const JointModel* joint_model : joint_model_vector_)
215  {
216  // the common root of a joint and itself is the same joint:
217  common_joint_roots_[joint_model->getJointIndex() * (1 + joint_model_vector_.size())] = joint_model->getJointIndex();
218 
219  // a node N and one of its descendants have as common root the node N itself:
220  const std::vector<const JointModel*>& d = joint_model->getDescendantJointModels();
221  for (const JointModel* descendant_joint_model : d)
222  common_joint_roots_[descendant_joint_model->getJointIndex() * joint_model_vector_.size() +
223  joint_model->getJointIndex()] =
224  common_joint_roots_[descendant_joint_model->getJointIndex() +
225  joint_model->getJointIndex() * joint_model_vector_.size()] = joint_model->getJointIndex();
226  }
227 }
228 
230 {
231  // compute the list of descendants for all joints
232  std::vector<const JointModel*> parents;
233  std::set<const JointModel*> seen;
234 
235  DescMap descendants;
236  computeDescendantsHelper(root_joint_, parents, seen, descendants);
237  for (std::pair<const JointModel* const, std::pair<std::set<const LinkModel*, OrderLinksByIndex>,
238  std::set<const JointModel*, OrderJointsByIndex>>>& descendant :
239  descendants)
240  {
241  JointModel* jm = const_cast<JointModel*>(descendant.first);
242  for (const JointModel* jt : descendant.second.second)
243  jm->addDescendantJointModel(jt);
244  for (const LinkModel* jt : descendant.second.first)
245  jm->addDescendantLinkModel(jt);
246  }
247 }
248 
250 {
252  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::buildJointInfo");
253 
254  // construct additional maps for easy access by name
255  variable_count_ = 0;
257  variable_names_.reserve(joint_model_vector_.size());
259 
260  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
261  {
262  joint_model_vector_[i]->setJointIndex(i);
263  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
264 
265  // compute index map
266  if (!name_order.empty())
267  {
268  for (std::size_t j = 0; j < name_order.size(); ++j)
269  {
270  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
271  variable_names_.push_back(name_order[j]);
273  }
274  if (joint_model_vector_[i]->getMimic() == nullptr)
275  {
281  }
282 
283  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
284  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
286 
287  joint_model_vector_[i]->setFirstVariableIndex(variable_count_);
289 
290  // compute variable count
291  std::size_t vc = joint_model_vector_[i]->getVariableCount();
292  variable_count_ += vc;
293  if (vc == 1)
295  else
297  }
298  }
299 
300  std::vector<bool> link_considered(link_model_vector_.size(), false);
301  for (const LinkModel* link : link_model_vector_)
302  {
303  if (link_considered[link->getLinkIndex()])
304  continue;
305 
306  LinkTransformMap associated_transforms;
307  computeFixedTransforms(link, link->getJointOriginTransform().inverse(), associated_transforms);
308  for (auto& tf_base : associated_transforms)
309  {
310  link_considered[tf_base.first->getLinkIndex()] = true;
311  for (auto& tf_target : associated_transforms)
312  {
313  if (&tf_base != &tf_target)
314  const_cast<LinkModel*>(tf_base.first) // regain write access to base LinkModel*
315  ->addAssociatedFixedTransform(tf_target.first, tf_base.second.inverse() * tf_target.second);
316  }
317  }
318  }
319 
321  computeCommonRoots(); // must be called _after_ list of descendants was computed
322 }
323 
324 void RobotModel::buildGroupStates(const srdf::Model& srdf_model)
325 {
326  // copy the default states to the groups
327  const std::vector<srdf::Model::GroupState>& ds = srdf_model.getGroupStates();
328  for (const srdf::Model::GroupState& group_state : ds)
329  {
330  if (hasJointModelGroup(group_state.group_))
331  {
332  JointModelGroup* jmg = getJointModelGroup(group_state.group_);
333  std::vector<const JointModel*> remaining_joints = jmg->getActiveJointModels();
334  std::map<std::string, double> state;
335  for (std::map<std::string, std::vector<double>>::const_iterator jt = group_state.joint_values_.begin();
336  jt != group_state.joint_values_.end(); ++jt)
337  {
338  if (jmg->hasJointModel(jt->first))
339  {
340  const JointModel* jm = jmg->getJointModel(jt->first);
341  const std::vector<std::string>& vn = jm->getVariableNames();
342  // Remove current joint name from remaining list.
343  auto it_found = std::find(remaining_joints.begin(), remaining_joints.end(), jm);
344  if (it_found != remaining_joints.end())
345  remaining_joints.erase(it_found);
346  if (vn.size() == jt->second.size())
347  for (std::size_t j = 0; j < vn.size(); ++j)
348  state[vn[j]] = jt->second[j];
349  else
351  "The model for joint '%s' requires %d variable values, "
352  "but only %d variable values were supplied in default state '%s' for group '%s'",
353  jt->first.c_str(), (int)vn.size(), (int)jt->second.size(), group_state.name_.c_str(),
354  jmg->getName().c_str());
355  }
356  else
358  "Group state '%s' specifies value for joint '%s', "
359  "but that joint is not part of group '%s'",
360  group_state.name_.c_str(), jt->first.c_str(), jmg->getName().c_str());
361  }
362  if (!remaining_joints.empty())
363  {
364  std::stringstream missing;
365  missing << (*remaining_joints.begin())->getName();
366  for (auto j = ++remaining_joints.begin(); j != remaining_joints.end(); j++)
367  {
368  missing << ", " << (*j)->getName();
369  }
370  ROS_WARN_STREAM_NAMED(LOGNAME, "Group state '" << group_state.name_
371  << "' doesn't specify all group joints in group '"
372  << group_state.group_ << "'. " << missing.str() << " "
373  << (remaining_joints.size() > 1 ? "are" : "is") << " missing.");
374  }
375  if (!state.empty())
376  jmg->addDefaultState(group_state.name_, state);
377  }
378  else
379  ROS_ERROR_NAMED(LOGNAME, "Group state '%s' specified for group '%s', but that group does not exist",
380  group_state.name_.c_str(), group_state.group_.c_str());
381  }
382 }
383 
384 void RobotModel::buildMimic(const urdf::ModelInterface& urdf_model)
385 {
386  // compute mimic joints
387  for (JointModel* joint_model : joint_model_vector_)
388  {
389  const urdf::Joint* jm = urdf_model.getJoint(joint_model->getName()).get();
390  if (jm)
391  if (jm->mimic)
392  {
393  JointModelMap::const_iterator jit = joint_model_map_.find(jm->mimic->joint_name);
394  if (jit != joint_model_map_.end())
395  {
396  if (joint_model->getVariableCount() == jit->second->getVariableCount())
397  joint_model->setMimic(jit->second, jm->mimic->multiplier, jm->mimic->offset);
398  else
399  ROS_ERROR_NAMED(LOGNAME, "Join '%s' cannot mimic joint '%s' because they have different number of DOF",
400  joint_model->getName().c_str(), jm->mimic->joint_name.c_str());
401  }
402  else
403  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' cannot mimic unknown joint '%s'", joint_model->getName().c_str(),
404  jm->mimic->joint_name.c_str());
405  }
406  }
407 
408  // in case we have a joint that mimics a joint that already mimics another joint, we can simplify things:
409  bool change = true;
410  while (change)
411  {
412  change = false;
413  for (JointModel* joint_model : joint_model_vector_)
414  if (joint_model->getMimic())
415  {
416  if (joint_model->getMimic()->getMimic())
417  {
418  joint_model->setMimic(joint_model->getMimic()->getMimic(),
419  joint_model->getMimicFactor() * joint_model->getMimic()->getMimicFactor(),
420  joint_model->getMimicOffset() +
421  joint_model->getMimicFactor() * joint_model->getMimic()->getMimicOffset());
422  change = true;
423  }
424  if (joint_model == joint_model->getMimic())
425  {
426  ROS_ERROR_NAMED(LOGNAME, "Cycle found in joint that mimic each other. Ignoring all mimic joints.");
427  for (JointModel* joint_model_recal : joint_model_vector_)
428  joint_model_recal->setMimic(nullptr, 0.0, 0.0);
429  change = false;
430  break;
431  }
432  }
433  }
434  // build mimic requests
435  for (JointModel* joint_model : joint_model_vector_)
436  if (joint_model->getMimic())
437  {
438  const_cast<JointModel*>(joint_model->getMimic())->addMimicRequest(joint_model);
439  mimic_joints_.push_back(joint_model);
440  }
441 }
442 
443 bool RobotModel::hasEndEffector(const std::string& eef) const
444 {
445  return end_effectors_map_.find(eef) != end_effectors_map_.end();
446 }
447 
448 const JointModelGroup* RobotModel::getEndEffector(const std::string& name) const
449 {
450  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
451  if (it == end_effectors_map_.end())
452  {
453  it = joint_model_group_map_.find(name);
454  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
455  return it->second;
456  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
457  return nullptr;
458  }
459  return it->second;
460 }
461 
462 JointModelGroup* RobotModel::getEndEffector(const std::string& name)
463 {
464  JointModelGroupMap::const_iterator it = end_effectors_map_.find(name);
465  if (it == end_effectors_map_.end())
466  {
467  it = joint_model_group_map_.find(name);
468  if (it != joint_model_group_map_.end() && it->second->isEndEffector())
469  return it->second;
470  ROS_ERROR_NAMED(LOGNAME, "End-effector '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
471  return nullptr;
472  }
473  return it->second;
474 }
475 
476 bool RobotModel::hasJointModelGroup(const std::string& name) const
477 {
478  return joint_model_group_map_.find(name) != joint_model_group_map_.end();
479 }
480 
481 const JointModelGroup* RobotModel::getJointModelGroup(const std::string& name) const
482 {
483  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
484  if (it == joint_model_group_map_.end())
485  {
486  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
487  return nullptr;
488  }
489  return it->second;
490 }
491 
492 JointModelGroup* RobotModel::getJointModelGroup(const std::string& name)
493 {
494  JointModelGroupMap::const_iterator it = joint_model_group_map_.find(name);
495  if (it == joint_model_group_map_.end())
496  {
497  ROS_ERROR_NAMED(LOGNAME, "Group '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
498  return nullptr;
499  }
500  return it->second;
501 }
502 
503 void RobotModel::buildGroups(const srdf::Model& srdf_model)
504 {
505  const std::vector<srdf::Model::Group>& group_configs = srdf_model.getGroups();
506 
507  // the only thing tricky is dealing with subgroups
508  std::vector<bool> processed(group_configs.size(), false);
509 
510  bool added = true;
511  while (added)
512  {
513  added = false;
514 
515  // going to make passes until we can't do anything else
516  for (std::size_t i = 0; i < group_configs.size(); ++i)
517  if (!processed[i])
518  {
519  // if we haven't processed, check and see if the dependencies are met yet
520  bool all_subgroups_added = true;
521  for (const std::string& subgroup : group_configs[i].subgroups_)
522  if (joint_model_group_map_.find(subgroup) == joint_model_group_map_.end())
523  {
524  all_subgroups_added = false;
525  break;
526  }
527  if (all_subgroups_added)
528  {
529  added = true;
530  processed[i] = true;
531  if (!addJointModelGroup(group_configs[i]))
532  ROS_WARN_NAMED(LOGNAME, "Failed to add group '%s'", group_configs[i].name_.c_str());
533  }
534  }
535  }
536 
537  for (std::size_t i = 0; i < processed.size(); ++i)
538  if (!processed[i])
539  ROS_WARN_NAMED(LOGNAME, "Could not process group '%s' due to unmet subgroup dependencies",
540  group_configs[i].name_.c_str());
541 
542  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
543  joint_model_groups_.push_back(it->second);
544  std::sort(joint_model_groups_.begin(), joint_model_groups_.end(), OrderGroupsByName());
545  for (JointModelGroup* joint_model_group : joint_model_groups_)
546  {
547  joint_model_groups_const_.push_back(joint_model_group);
548  joint_model_group_names_.push_back(joint_model_group->getName());
549  }
550 
551  buildGroupsInfoSubgroups(srdf_model);
552  buildGroupsInfoEndEffectors(srdf_model);
553 }
554 
556 {
557  // compute subgroups
558  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
559  {
560  JointModelGroup* jmg = it->second;
561  std::vector<std::string> subgroup_names;
562  std::set<const JointModel*> joints(jmg->getJointModels().begin(), jmg->getJointModels().end());
563  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
564  ++jt)
565  if (jt->first != it->first)
566  {
567  bool ok = true;
568  JointModelGroup* sub_jmg = jt->second;
569  const std::vector<const JointModel*>& sub_joints = sub_jmg->getJointModels();
570  for (const JointModel* sub_joint : sub_joints)
571  if (joints.find(sub_joint) == joints.end())
572  {
573  ok = false;
574  break;
575  }
576  if (ok)
577  subgroup_names.push_back(sub_jmg->getName());
578  }
579  if (!subgroup_names.empty())
580  jmg->setSubgroupNames(subgroup_names);
581  }
582 }
583 
585 {
586  // set the end-effector flags
587  const std::vector<srdf::Model::EndEffector>& eefs = srdf_model.getEndEffectors();
588  for (JointModelGroupMap::const_iterator it = joint_model_group_map_.begin(); it != joint_model_group_map_.end(); ++it)
589  {
590  // check if this group is a known end effector
591  for (const srdf::Model::EndEffector& eef : eefs)
592  if (eef.component_group_ == it->first)
593  {
594  // if it is, mark it as such
595  it->second->setEndEffectorName(eef.name_);
596  end_effectors_map_[eef.name_] = it->second;
597  end_effectors_.push_back(it->second);
598 
599  // check to see if there are groups that contain the parent link of this end effector.
600  // record this information if found;
601  std::vector<JointModelGroup*> possible_parent_groups;
602  for (JointModelGroupMap::const_iterator jt = joint_model_group_map_.begin(); jt != joint_model_group_map_.end();
603  ++jt)
604  if (jt->first != it->first)
605  {
606  if (jt->second->hasLinkModel(eef.parent_link_))
607  {
608  jt->second->attachEndEffector(eef.name_);
609  possible_parent_groups.push_back(jt->second);
610  }
611  }
612 
613  JointModelGroup* eef_parent_group = nullptr;
614  // if a parent group is specified in SRDF, try to use it
615  if (!eef.parent_group_.empty())
616  {
617  JointModelGroupMap::const_iterator jt = joint_model_group_map_.find(eef.parent_group_);
618  if (jt != joint_model_group_map_.end())
619  {
620  if (jt->second->hasLinkModel(eef.parent_link_))
621  {
622  if (jt->second != it->second)
623  eef_parent_group = jt->second;
624  else
625  ROS_ERROR_NAMED(LOGNAME, "Group '%s' for end-effector '%s' cannot be its own parent",
626  eef.parent_group_.c_str(), eef.name_.c_str());
627  }
628  else
630  "Group '%s' was specified as parent group for end-effector '%s' "
631  "but it does not include the parent link '%s'",
632  eef.parent_group_.c_str(), eef.name_.c_str(), eef.parent_link_.c_str());
633  }
634  else
635  ROS_ERROR_NAMED(LOGNAME, "Group name '%s' not found (specified as parent group for end-effector '%s')",
636  eef.parent_group_.c_str(), eef.name_.c_str());
637  }
638 
639  // if no parent group was specified, use a default one
640  if (eef_parent_group == nullptr)
641  if (!possible_parent_groups.empty())
642  {
643  // if there are multiple options for the group that contains this end-effector,
644  // we pick the group with fewest joints.
645  std::size_t best = 0;
646  for (std::size_t g = 1; g < possible_parent_groups.size(); ++g)
647  if (possible_parent_groups[g]->getJointModels().size() <
648  possible_parent_groups[best]->getJointModels().size())
649  best = g;
650  eef_parent_group = possible_parent_groups[best];
651  }
652 
653  if (eef_parent_group)
654  {
655  it->second->setEndEffectorParent(eef_parent_group->getName(), eef.parent_link_);
656  }
657  else
658  {
659  ROS_WARN_NAMED(LOGNAME, "Could not identify parent group for end-effector '%s'", eef.name_.c_str());
660  it->second->setEndEffectorParent("", eef.parent_link_);
661  }
662  }
663  }
664  std::sort(end_effectors_.begin(), end_effectors_.end(), OrderGroupsByName());
665 }
666 
668 {
670  {
671  ROS_WARN_NAMED(LOGNAME, "A group named '%s' already exists. Not adding.", gc.name_.c_str());
672  return false;
673  }
674 
675  std::set<const JointModel*> jset;
676 
677  // add joints from chains
678  for (const std::pair<std::string, std::string>& chain : gc.chains_)
679  {
680  const LinkModel* base_link = getLinkModel(chain.first);
681  const LinkModel* tip_link = getLinkModel(chain.second);
682  if (base_link && tip_link)
683  {
684  // go from tip, up the chain, until we hit the root or we find the base_link
685  const LinkModel* lm = tip_link;
686  std::vector<const JointModel*> cj;
687  while (lm)
688  {
689  if (lm == base_link)
690  break;
691  cj.push_back(lm->getParentJointModel());
693  }
694  // if we did not find the base_link, we could have a chain like e.g.,
695  // from one end-effector to another end-effector, so the root is in between
696  if (lm != base_link)
697  {
698  // we go up the chain from the base this time, and see where we intersect the other chain
699  lm = base_link;
700  std::size_t index = 0;
701  std::vector<const JointModel*> cj2;
702  while (lm)
703  {
704  for (std::size_t j = 0; j < cj.size(); ++j)
705  if (cj[j] == lm->getParentJointModel())
706  {
707  index = j + 1;
708  break;
709  }
710  if (index > 0)
711  break;
712  cj2.push_back(lm->getParentJointModel());
713  lm = lm->getParentJointModel()->getParentLinkModel();
714  }
715  if (index > 0)
716  {
717  jset.insert(cj.begin(), cj.begin() + index);
718  jset.insert(cj2.begin(), cj2.end());
719  }
720  }
721  else
722  // if we have a simple chain, just add the joints
723  jset.insert(cj.begin(), cj.end());
724  }
725  }
726 
727  // add joints
728  for (const std::string& joint : gc.joints_)
729  {
730  const JointModel* j = getJointModel(joint);
731  if (j)
732  jset.insert(j);
733  }
734 
735  // add joints that are parents of included links
736  for (const std::string& link : gc.links_)
737  {
738  const LinkModel* l = getLinkModel(link);
739  if (l)
740  jset.insert(l->getParentJointModel());
741  }
742 
743  // add joints from subgroups
744  for (const std::string& subgroup : gc.subgroups_)
745  {
746  const JointModelGroup* sg = getJointModelGroup(subgroup);
747  if (sg)
748  {
749  // active joints
750  const std::vector<const JointModel*>& js = sg->getJointModels();
751  for (const JointModel* j : js)
752  jset.insert(j);
753 
754  // fixed joints
755  const std::vector<const JointModel*>& fs = sg->getFixedJointModels();
756  for (const JointModel* f : fs)
757  jset.insert(f);
758 
759  // mimic joints
760  const std::vector<const JointModel*>& ms = sg->getMimicJointModels();
761  for (const JointModel* m : ms)
762  jset.insert(m);
763  }
764  }
765 
766  if (jset.empty())
767  {
768  ROS_WARN_NAMED(LOGNAME, "Group '%s' must have at least one valid joint", gc.name_.c_str());
769  return false;
770  }
771 
772  std::vector<const JointModel*> joints;
773  joints.reserve(jset.size());
774  for (const JointModel* it : jset)
775  joints.push_back(it);
776 
777  JointModelGroup* jmg = new JointModelGroup(gc.name_, gc, joints, this);
778  joint_model_group_map_[gc.name_] = jmg;
779 
780  return true;
781 }
782 
783 JointModel* RobotModel::buildRecursive(LinkModel* parent, const urdf::Link* urdf_link, const srdf::Model& srdf_model)
784 {
785  // construct the joint
786  JointModel* joint = urdf_link->parent_joint ?
787  constructJointModel(urdf_link->parent_joint.get(), urdf_link, srdf_model) :
788  constructJointModel(nullptr, urdf_link, srdf_model);
789  if (joint == nullptr)
790  return nullptr;
791 
792  // bookkeeping for the joint
793  joint_model_map_[joint->getName()] = joint;
794  joint->setJointIndex(joint_model_vector_.size());
795  joint_model_vector_.push_back(joint);
796  joint_model_vector_const_.push_back(joint);
797  joint_model_names_vector_.push_back(joint->getName());
798  joint->setParentLinkModel(parent);
799 
800  // construct the link
801  LinkModel* link = constructLinkModel(urdf_link);
802  joint->setChildLinkModel(link);
803  link->setParentLinkModel(parent);
804 
805  // bookkeeping for the link
806  link_model_map_[joint->getChildLinkModel()->getName()] = link;
807  link->setLinkIndex(link_model_vector_.size());
808  link_model_vector_.push_back(link);
809  link_model_vector_const_.push_back(link);
810  link_model_names_vector_.push_back(link->getName());
811  if (!link->getShapes().empty())
812  {
814  link_model_names_with_collision_geometry_vector_.push_back(link->getName());
815  link->setFirstCollisionBodyTransformIndex(link_geometry_count_);
816  link_geometry_count_ += link->getShapes().size();
817  }
818  link->setParentJointModel(joint);
819 
820  // recursively build child links (and joints)
821  for (const urdf::LinkSharedPtr& child_link : urdf_link->child_links)
822  {
823  JointModel* jm = buildRecursive(link, child_link.get(), srdf_model);
824  if (jm)
825  link->addChildJointModel(jm);
826  }
827  return joint;
828 }
829 
830 namespace
831 {
832 // construct bounds for 1DOF joint
833 static inline VariableBounds jointBoundsFromURDF(const urdf::Joint* urdf_joint)
834 {
835  VariableBounds b;
836  if (urdf_joint->safety)
837  {
838  b.position_bounded_ = true;
839  b.min_position_ = urdf_joint->safety->soft_lower_limit;
840  b.max_position_ = urdf_joint->safety->soft_upper_limit;
841  if (urdf_joint->limits)
842  {
843  if (urdf_joint->limits->lower > b.min_position_)
844  b.min_position_ = urdf_joint->limits->lower;
845  if (urdf_joint->limits->upper < b.max_position_)
846  b.max_position_ = urdf_joint->limits->upper;
847  }
848  }
849  else
850  {
851  if (urdf_joint->limits)
852  {
853  b.position_bounded_ = true;
854  b.min_position_ = urdf_joint->limits->lower;
855  b.max_position_ = urdf_joint->limits->upper;
856  }
857  }
858  if (urdf_joint->limits)
859  {
860  b.max_velocity_ = fabs(urdf_joint->limits->velocity);
861  b.min_velocity_ = -b.max_velocity_;
862  b.velocity_bounded_ = b.max_velocity_ > std::numeric_limits<double>::epsilon();
863  }
864  return b;
865 }
866 } // namespace
867 
868 JointModel* RobotModel::constructJointModel(const urdf::Joint* urdf_joint, const urdf::Link* child_link,
869  const srdf::Model& srdf_model)
870 {
871  JointModel* new_joint_model = nullptr;
872 
873  // if urdf_joint exists, must be the root link transform
874  if (urdf_joint)
875  {
876  switch (urdf_joint->type)
877  {
878  case urdf::Joint::REVOLUTE:
879  {
880  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
881  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
882  j->setContinuous(false);
883  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
884  new_joint_model = j;
885  }
886  break;
887  case urdf::Joint::CONTINUOUS:
888  {
889  RevoluteJointModel* j = new RevoluteJointModel(urdf_joint->name);
890  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
891  j->setContinuous(true);
892  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
893  new_joint_model = j;
894  }
895  break;
896  case urdf::Joint::PRISMATIC:
897  {
898  PrismaticJointModel* j = new PrismaticJointModel(urdf_joint->name);
899  j->setVariableBounds(j->getName(), jointBoundsFromURDF(urdf_joint));
900  j->setAxis(Eigen::Vector3d(urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z));
901  new_joint_model = j;
902  }
903  break;
904  case urdf::Joint::FLOATING:
905  new_joint_model = new FloatingJointModel(urdf_joint->name);
906  break;
907  case urdf::Joint::PLANAR:
908  new_joint_model = new PlanarJointModel(urdf_joint->name);
909  break;
910  case urdf::Joint::FIXED:
911  new_joint_model = new FixedJointModel(urdf_joint->name);
912  break;
913  default:
914  ROS_ERROR_NAMED(LOGNAME, "Unknown joint type: %d", (int)urdf_joint->type);
915  break;
916  }
917  }
918  else // if urdf_joint passed in as null, then we're at root of URDF model
919  {
920  const std::vector<srdf::Model::VirtualJoint>& virtual_joints = srdf_model.getVirtualJoints();
921  for (const srdf::Model::VirtualJoint& virtual_joint : virtual_joints)
922  {
923  if (virtual_joint.child_link_ != child_link->name)
924  {
926  "Skipping virtual joint '%s' because its child frame '%s' "
927  "does not match the URDF frame '%s'",
928  virtual_joint.name_.c_str(), virtual_joint.child_link_.c_str(), child_link->name.c_str());
929  }
930  else if (virtual_joint.parent_frame_.empty())
931  {
932  ROS_WARN_NAMED(LOGNAME, "Skipping virtual joint '%s' because its parent frame is empty",
933  virtual_joint.name_.c_str());
934  }
935  else
936  {
937  if (virtual_joint.type_ == "fixed")
938  new_joint_model = new FixedJointModel(virtual_joint.name_);
939  else if (virtual_joint.type_ == "planar")
940  new_joint_model = new PlanarJointModel(virtual_joint.name_);
941  else if (virtual_joint.type_ == "floating")
942  new_joint_model = new FloatingJointModel(virtual_joint.name_);
943  if (new_joint_model)
944  {
945  // for fixed frames we still use the robot root link
946  if (virtual_joint.type_ != "fixed")
947  {
948  model_frame_ = virtual_joint.parent_frame_;
949  }
950  break;
951  }
952  }
953  }
954  if (!new_joint_model)
955  {
956  ROS_INFO_NAMED(LOGNAME, "No root/virtual joint specified in SRDF. Assuming fixed joint");
957  new_joint_model = new FixedJointModel("ASSUMED_FIXED_ROOT_JOINT");
958  }
959  }
960 
961  if (new_joint_model)
962  {
963  new_joint_model->setDistanceFactor(new_joint_model->getStateSpaceDimension());
964  const std::vector<srdf::Model::PassiveJoint>& pjoints = srdf_model.getPassiveJoints();
965  for (const srdf::Model::PassiveJoint& pjoint : pjoints)
966  {
967  if (new_joint_model->getName() == pjoint.name_)
968  {
969  new_joint_model->setPassive(true);
970  break;
971  }
972  }
973  }
974 
975  return new_joint_model;
976 }
977 
978 namespace
979 {
980 static inline Eigen::Isometry3d urdfPose2Isometry3d(const urdf::Pose& pose)
981 {
982  Eigen::Quaterniond q(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z);
983  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
984  return af;
985 }
986 } // namespace
987 
988 LinkModel* RobotModel::constructLinkModel(const urdf::Link* urdf_link)
989 {
990  LinkModel* new_link_model = new LinkModel(urdf_link->name);
991 
992  const std::vector<urdf::CollisionSharedPtr>& col_array =
993  urdf_link->collision_array.empty() ? std::vector<urdf::CollisionSharedPtr>(1, urdf_link->collision) :
994  urdf_link->collision_array;
995 
996  std::vector<shapes::ShapeConstPtr> shapes;
997  EigenSTL::vector_Isometry3d poses;
998 
999  for (const urdf::CollisionSharedPtr& col : col_array)
1000  {
1001  if (col && col->geometry)
1002  {
1003  shapes::ShapeConstPtr s = constructShape(col->geometry.get());
1004  if (s)
1005  {
1006  shapes.push_back(s);
1007  poses.push_back(urdfPose2Isometry3d(col->origin));
1008  }
1009  }
1010  }
1011 
1012  // Should we warn that old (melodic) behaviour has changed, not copying visual to collision geometries anymore?
1013  bool warn_about_missing_collision = false;
1014  if (shapes.empty())
1015  {
1016  const auto& vis_array = urdf_link->visual_array.empty() ? std::vector<urdf::VisualSharedPtr>{ urdf_link->visual } :
1017  urdf_link->visual_array;
1018  for (const urdf::VisualSharedPtr& vis : vis_array)
1019  {
1020  if (vis && vis->geometry)
1021  warn_about_missing_collision = true;
1022  }
1023  }
1024  if (warn_about_missing_collision)
1025  {
1026  ROS_WARN_STREAM_NAMED(LOGNAME + ".empty_collision_geometry",
1027  "Link " << urdf_link->name
1028  << " has visual geometry but no collision geometry. "
1029  "Collision geometry will be left empty. "
1030  "Fix your URDF file by explicitly specifying collision geometry.");
1031  }
1032 
1033  new_link_model->setGeometry(shapes, poses);
1034 
1035  // figure out visual mesh (try visual urdf tag first, collision tag otherwise
1036  if (urdf_link->visual && urdf_link->visual->geometry)
1037  {
1038  if (urdf_link->visual->geometry->type == urdf::Geometry::MESH)
1039  {
1040  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->visual->geometry.get());
1041  if (!mesh->filename.empty())
1042  new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->visual->origin),
1043  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1044  }
1045  }
1046  else if (urdf_link->collision && urdf_link->collision->geometry)
1047  {
1048  if (urdf_link->collision->geometry->type == urdf::Geometry::MESH)
1049  {
1050  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(urdf_link->collision->geometry.get());
1051  if (!mesh->filename.empty())
1052  new_link_model->setVisualMesh(mesh->filename, urdfPose2Isometry3d(urdf_link->collision->origin),
1053  Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z));
1054  }
1055  }
1056 
1057  if (urdf_link->parent_joint)
1058  new_link_model->setJointOriginTransform(
1059  urdfPose2Isometry3d(urdf_link->parent_joint->parent_to_joint_origin_transform));
1060 
1061  return new_link_model;
1062 }
1063 
1064 shapes::ShapePtr RobotModel::constructShape(const urdf::Geometry* geom)
1065 {
1066  moveit::tools::Profiler::ScopedBlock prof_block("RobotModel::constructShape");
1067 
1068  shapes::Shape* new_shape = nullptr;
1069  switch (geom->type)
1070  {
1071  case urdf::Geometry::SPHERE:
1072  new_shape = new shapes::Sphere(static_cast<const urdf::Sphere*>(geom)->radius);
1073  break;
1074  case urdf::Geometry::BOX:
1075  {
1076  urdf::Vector3 dim = static_cast<const urdf::Box*>(geom)->dim;
1077  new_shape = new shapes::Box(dim.x, dim.y, dim.z);
1078  }
1079  break;
1080  case urdf::Geometry::CYLINDER:
1081  new_shape = new shapes::Cylinder(static_cast<const urdf::Cylinder*>(geom)->radius,
1082  static_cast<const urdf::Cylinder*>(geom)->length);
1083  break;
1084  case urdf::Geometry::MESH:
1085  {
1086  const urdf::Mesh* mesh = static_cast<const urdf::Mesh*>(geom);
1087  if (!mesh->filename.empty())
1088  {
1089  Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
1090  shapes::Mesh* m = shapes::createMeshFromResource(mesh->filename, scale);
1091  new_shape = m;
1092  }
1093  }
1094  break;
1095  default:
1096  ROS_ERROR_NAMED(LOGNAME, "Unknown geometry type: %d", (int)geom->type);
1097  break;
1098  }
1099 
1100  return shapes::ShapePtr(new_shape);
1101 }
1102 
1103 bool RobotModel::hasJointModel(const std::string& name) const
1104 {
1105  return joint_model_map_.find(name) != joint_model_map_.end();
1106 }
1107 
1108 bool RobotModel::hasLinkModel(const std::string& name) const
1109 {
1110  return link_model_map_.find(name) != link_model_map_.end();
1111 }
1112 
1113 const JointModel* RobotModel::getJointModel(const std::string& name) const
1114 {
1115  JointModelMap::const_iterator it = joint_model_map_.find(name);
1116  if (it != joint_model_map_.end())
1117  return it->second;
1118  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1119  return nullptr;
1120 }
1121 
1122 const JointModel* RobotModel::getJointModel(int index) const
1123 {
1124  if (index < 0 || index >= static_cast<int>(joint_model_vector_.size()))
1125  {
1126  ROS_ERROR_NAMED(LOGNAME, "Joint index '%i' out of bounds of joints in model '%s'", index, model_name_.c_str());
1127  return nullptr;
1128  }
1129  assert(joint_model_vector_[index]->getJointIndex() == index);
1131 }
1132 
1133 JointModel* RobotModel::getJointModel(const std::string& name)
1134 {
1135  JointModelMap::const_iterator it = joint_model_map_.find(name);
1136  if (it != joint_model_map_.end())
1137  return it->second;
1138  ROS_ERROR_NAMED(LOGNAME, "Joint '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1139  return nullptr;
1140 }
1141 
1142 const LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link) const
1143 {
1144  return const_cast<RobotModel*>(this)->getLinkModel(name, has_link);
1145 }
1146 
1147 const LinkModel* RobotModel::getLinkModel(int index) const
1148 {
1149  if (index < 0 || index >= static_cast<int>(link_model_vector_.size()))
1150  {
1151  ROS_ERROR_NAMED(LOGNAME, "Link index '%i' out of bounds of links in model '%s'", index, model_name_.c_str());
1152  return nullptr;
1153  }
1154  assert(link_model_vector_[index]->getLinkIndex() == index);
1155  return link_model_vector_[index];
1156 }
1157 
1158 LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
1159 {
1160  if (has_link)
1161  *has_link = true; // Start out optimistic
1162  LinkModelMap::const_iterator it = link_model_map_.find(name);
1163  if (it != link_model_map_.end())
1164  return it->second;
1165 
1166  if (has_link)
1167  *has_link = false; // Report failure via argument
1168  else // Otherwise print error
1169  ROS_ERROR_NAMED(LOGNAME, "Link '%s' not found in model '%s'", name.c_str(), model_name_.c_str());
1170  return nullptr;
1171 }
1172 
1175  if (!link)
1176  return link;
1177  const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
1178  const moveit::core::JointModel* joint = link->getParentJointModel();
1180  while (parent_link && joint->getType() == moveit::core::JointModel::FIXED)
1181  {
1182  link = parent_link;
1183  joint = link->getParentJointModel();
1184  parent_link = joint->getParentLinkModel();
1185  }
1186  return link;
1187 }
1189 void RobotModel::updateMimicJoints(double* values) const
1190 {
1191  for (const JointModel* mimic_joint : mimic_joints_)
1192  {
1193  int src = mimic_joint->getMimic()->getFirstVariableIndex();
1194  int dest = mimic_joint->getFirstVariableIndex();
1195  values[dest] = values[src] * mimic_joint->getMimicFactor() + mimic_joint->getMimicOffset();
1196  }
1197 }
1198 
1200 {
1201  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1204 }
1205 
1207  std::map<std::string, double>& values) const
1209  std::vector<double> tmp(variable_count_);
1210  getVariableRandomPositions(rng, &tmp[0]);
1211  values.clear();
1212  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1213  values[variable_names_[i]] = tmp[i];
1214 }
1215 
1216 void RobotModel::getVariableDefaultPositions(double* values) const
1217 {
1218  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1221 }
1222 
1223 void RobotModel::getVariableDefaultPositions(std::map<std::string, double>& values) const
1225  std::vector<double> tmp(variable_count_);
1226  getVariableDefaultPositions(&tmp[0]);
1227  values.clear();
1228  for (std::size_t i = 0; i < variable_names_.size(); ++i)
1229  values[variable_names_[i]] = tmp[i];
1230 }
1231 
1232 void RobotModel::getMissingVariableNames(const std::vector<std::string>& variables,
1233  std::vector<std::string>& missing_variables) const
1234 {
1235  missing_variables.clear();
1236  std::set<std::string> keys(variables.begin(), variables.end());
1237  for (const std::string& variable_name : variable_names_)
1238  if (keys.find(variable_name) == keys.end())
1239  if (getJointOfVariable(variable_name)->getMimic() == nullptr)
1240  missing_variables.push_back(variable_name);
1241 }
1242 
1243 int RobotModel::getVariableIndex(const std::string& variable) const
1244 {
1245  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
1246  if (it == joint_variables_index_map_.end())
1247  throw Exception("Variable '" + variable + "' is not known to model '" + model_name_ + "'");
1248  return it->second;
1249 }
1250 
1251 double RobotModel::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
1252 {
1253  double max_distance = 0.0;
1254  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
1255  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
1256  active_joint_model_vector_[j]->getDistanceFactor();
1257  return max_distance;
1258 }
1259 
1260 bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
1261  double margin) const
1262 {
1263  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1264  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1266  *active_joint_bounds[i], margin))
1267  return false;
1268  return true;
1269 }
1270 
1271 bool RobotModel::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
1273  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
1274  bool change = false;
1275  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1277  *active_joint_bounds[i]))
1278  change = true;
1279  if (change)
1280  updateMimicJoints(state);
1281  return change;
1283 
1284 double RobotModel::distance(const double* state1, const double* state2) const
1285 {
1286  double d = 0.0;
1287  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1288  d += active_joint_model_vector_[i]->getDistanceFactor() *
1290  state2 + active_joint_model_start_index_[i]);
1291  return d;
1292 }
1293 
1294 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
1295 {
1297  // we interpolate values only for active joint models (non-mimic)
1298  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
1301  state + active_joint_model_start_index_[i]);
1302  // now we update mimic as needed
1303  updateMimicJoints(state);
1304 }
1305 
1306 void RobotModel::setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators)
1307 {
1308  // we first set all the "simple" allocators -- where a group has one IK solver
1310  {
1311  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1312  if (jt != allocators.end())
1313  {
1314  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> solver_allocator_pair;
1315  solver_allocator_pair.first = jt->second;
1316  jmg->setSolverAllocators(solver_allocator_pair);
1317  }
1318  }
1319 
1320  // now we set compound IK solvers; we do this later because we need the index maps computed by the previous calls to
1321  // setSolverAllocators()
1322  for (JointModelGroup* jmg : joint_model_groups_)
1323  {
1324  std::pair<SolverAllocatorFn, SolverAllocatorMapFn> solver_allocator_pair;
1325  std::map<std::string, SolverAllocatorFn>::const_iterator jt = allocators.find(jmg->getName());
1326  if (jt == allocators.end())
1327  {
1328  // if an kinematics allocator is NOT available for this group, we try to see if we can use subgroups for IK
1329  std::set<const JointModel*> joints;
1330  joints.insert(jmg->getJointModels().begin(), jmg->getJointModels().end());
1331 
1332  std::vector<const JointModelGroup*> subs;
1333 
1334  // go through the groups that have IK allocators and see if they are part of jmg; collect them in subs
1335  for (const std::pair<const std::string, SolverAllocatorFn>& allocator : allocators)
1336  {
1337  const JointModelGroup* sub = getJointModelGroup(allocator.first);
1338  if (!sub) // this should actually not happen, all groups should be well defined
1339  {
1340  subs.clear();
1341  break;
1342  }
1343  std::set<const JointModel*> sub_joints;
1344  sub_joints.insert(sub->getJointModels().begin(), sub->getJointModels().end());
1345 
1346  if (std::includes(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end()))
1347  { // sub_joints included in joints: add sub, remove sub_joints from joints set
1348  std::set<const JointModel*> joint_model_set;
1349  std::set_difference(joints.begin(), joints.end(), sub_joints.begin(), sub_joints.end(),
1350  std::inserter(joint_model_set, joint_model_set.end()));
1351  // TODO: instead of maintaining disjoint joint sets here,
1352  // should we leave that work to JMG's setSolverAllocators() / computeIKIndexBijection()?
1353  // There, a disjoint bijection from joints to solvers is computed anyway.
1354  // Underlying question: How do we resolve overlaps? Now the first considered sub group "wins"
1355  // But, if the overlap only involves fixed joints, we could consider all sub groups
1356  subs.push_back(sub);
1357  joints.swap(joint_model_set);
1358  }
1359  }
1361  // if we found subgroups, pass that information to the planning group
1362  if (!subs.empty())
1363  {
1364  std::stringstream ss;
1365  for (const JointModelGroup* sub : subs)
1366  {
1367  ss << sub->getName() << " ";
1368  solver_allocator_pair.second[sub] = allocators.find(sub->getName())->second;
1369  }
1370  ROS_DEBUG_NAMED(LOGNAME, "Added sub-group IK allocators for group '%s': [ %s]", jmg->getName().c_str(),
1371  ss.str().c_str());
1372  }
1373  jmg->setSolverAllocators(solver_allocator_pair);
1374  }
1375  }
1376 }
1377 
1378 void RobotModel::printModelInfo(std::ostream& out) const
1379 {
1380  out << "Model " << model_name_ << " in frame " << model_frame_ << ", using " << getVariableCount() << " variables"
1381  << std::endl;
1382 
1383  std::ios_base::fmtflags old_flags = out.flags();
1384  out.setf(std::ios::fixed, std::ios::floatfield);
1385  std::streamsize old_prec = out.precision();
1386  out.precision(5);
1387  out << "Joints: " << std::endl;
1388  for (JointModel* joint_model : joint_model_vector_)
1389  {
1390  out << " '" << joint_model->getName() << "' (" << joint_model->getTypeName() << ")" << std::endl;
1391  out << " * Joint Index: " << joint_model->getJointIndex() << std::endl;
1392  const std::vector<std::string>& vn = joint_model->getVariableNames();
1393  out << " * " << vn.size() << (vn.size() > 1 ? " variables:" : (vn.empty() ? " variables" : " variable:"))
1394  << std::endl;
1395  int idx = joint_model->getFirstVariableIndex();
1396  for (const std::string& it : vn)
1397  {
1398  out << " * '" << it << "', index " << idx++ << " in full state";
1399  if (joint_model->getMimic())
1400  out << ", mimic '" << joint_model->getMimic()->getName() << "'";
1401  if (joint_model->isPassive())
1402  out << ", passive";
1403  out << std::endl;
1404  out << " " << joint_model->getVariableBounds(it) << std::endl;
1405  }
1406  }
1407  out << std::endl;
1408  out.precision(old_prec);
1409  out.flags(old_flags);
1410  out << "Links: " << std::endl;
1411  for (LinkModel* link_model : link_model_vector_)
1412  {
1413  out << " '" << link_model->getName() << "' with " << link_model->getShapes().size() << " geoms" << std::endl;
1414  if (link_model->parentJointIsFixed())
1415  out << " * "
1416  << "parent joint is fixed" << std::endl;
1417  if (link_model->jointOriginTransformIsIdentity())
1418  out << " * "
1419  << "joint origin transform is identity" << std::endl;
1420  }
1421 
1422  out << "Available groups: " << std::endl;
1423  for (JointModelGroup* joint_model_group : joint_model_groups_)
1424  joint_model_group->printGroupInfo(out);
1425 }
1426 
1427 void RobotModel::computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
1428  LinkTransformMap& associated_transforms)
1429 {
1430  associated_transforms[link] = transform * link->getJointOriginTransform();
1431  for (std::size_t i = 0; i < link->getChildJointModels().size(); ++i)
1432  if (link->getChildJointModels()[i]->getType() == JointModel::FIXED)
1433  computeFixedTransforms(link->getChildJointModels()[i]->getChildLinkModel(),
1434  transform * link->getJointOriginTransform(), associated_transforms);
1435 }
1436 
1437 } // end of namespace core
1438 } // 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:631
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:1179
moveit::core::RobotModel::end_effectors_
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:664
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:597
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:603
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:1372
moveit::core::RobotModel::constructLinkModel
LinkModel * constructLinkModel(const urdf::Link *urdf_link)
Given a urdf link, build the corresponding LinkModel object.
Definition: robot_model.cpp:1054
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:591
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:733
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:658
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:577
moveit::core::RobotModel::computeDescendants
void computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
Definition: robot_model.cpp:295
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:594
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:499
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:606
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:141
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:588
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:649
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:484
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:849
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:264
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:661
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:655
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:547
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:450
moveit::core::RobotModel::interpolate
void interpolate(const double *from, const double *to, double t, double *state) const
Definition: robot_model.cpp:1360
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:621
moveit::core::RobotModel::joint_model_map_
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:585
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:611
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:559
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:565
moveit::core::RobotModel::srdf_
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:549
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:562
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:416
moveit::core::RobotModel::hasEndEffector
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
Definition: robot_model.cpp:509
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:571
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:622
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:609
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:547
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:613
moveit::core::RobotModel::root_link_
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:556
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:568
moveit::core::RobotModel::urdf_
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:551
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:123
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:279
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:650
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:1493
moveit::core::RobotModel::hasJointModelGroup
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
Definition: robot_model.cpp:542
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:644
moveit::core::RobotModel::satisfiesPositionBounds
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:410
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:641
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
moveit::core::RobotModel::enforcePositionBounds
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:405
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:934
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:574
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:628
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:390
moveit::core::RobotModel::getRootLink
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:146
moveit::core::RobotModel::buildJointInfo
void buildJointInfo()
Compute helpful information about joints.
Definition: robot_model.cpp:315
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:638
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:151
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:1444
moveit::core::JointModelGroup::setSolverAllocators
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
Definition: joint_model_group.h:594
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:1208
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:334
moveit::core::RobotModel::getMissingVariableNames
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
Definition: robot_model.cpp:1298
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:1239
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:543
moveit::core::RobotModel::active_joint_model_names_vector_
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:600
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:1309
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
Return the sum of joint distances between two states. Only considers active joints.
Definition: robot_model.cpp:1350
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:216
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:1169
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:1130
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:636
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:582
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:569
moveit::core::RobotModel::getVariableDefaultPositions
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
Definition: robot_model.cpp:1282
moveit::core::checkInterpolationParamBounds
static void checkInterpolationParamBounds(const char LOGNAME[], double t)
Definition: robot_model.h:130
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:1265
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:1255
moveit::core::RobotModel::~RobotModel
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:131
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:151
t
geometry_msgs::TransformStamped t
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:1174
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:514
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:652


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 21 2021 02:23:42