joint_model_group.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, Dave Coleman */
37 
42 #include <console_bridge/console.h>
43 #include <boost/lexical_cast.hpp>
44 #include <algorithm>
45 #include "order_robot_model_items.inc"
46 
47 namespace moveit
48 {
49 namespace core
50 {
51 namespace
52 {
53 // check if a parent or ancestor of joint is included in this group
54 bool includesParent(const JointModel* joint, const JointModelGroup* group)
55 {
56  bool found = false;
57  // if we find that an ancestor is also in the group, then the joint is not a root
58  while (joint->getParentLinkModel() != NULL)
59  {
60  joint = joint->getParentLinkModel()->getParentJointModel();
61  if (group->hasJointModel(joint->getName()) && joint->getVariableCount() > 0 && joint->getMimic() == NULL)
62  {
63  found = true;
64  break;
65  }
66  else if (joint->getMimic() != NULL)
67  {
68  const JointModel* mjoint = joint->getMimic();
69  if (group->hasJointModel(mjoint->getName()) && mjoint->getVariableCount() > 0 && mjoint->getMimic() == NULL)
70  found = true;
71  else if (includesParent(mjoint, group))
72  found = true;
73  if (found)
74  break;
75  }
76  }
77  return found;
78 }
79 
80 // check if joint a is right below b, in the kinematic chain, with no active DOF missing
81 bool jointPrecedes(const JointModel* a, const JointModel* b)
82 {
83  if (!a->getParentLinkModel())
84  return false;
85  const JointModel* p = a->getParentLinkModel()->getParentJointModel();
86  while (p)
87  {
88  if (p == b)
89  return true;
90  if (p->getType() == JointModel::FIXED)
91  p = p->getParentLinkModel() ? p->getParentLinkModel()->getParentJointModel() : NULL;
92  else
93  break;
94  }
95 
96  return false;
97 }
98 }
99 }
100 }
101 
102 moveit::core::JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config,
103  const std::vector<const JointModel*>& unsorted_group_joints,
104  const RobotModel* parent_model)
105  : parent_model_(parent_model)
106  , name_(group_name)
107  , common_root_(NULL)
108  , variable_count_(0)
109  , is_contiguous_index_list_(true)
110  , is_chain_(false)
111  , is_single_dof_(true)
112  , config_(config)
113 {
114  // sort joints in Depth-First order
115  joint_model_vector_ = unsorted_group_joints;
116  std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
118 
119  // figure out active joints, mimic joints, fixed joints
120  // construct index maps, list of variables
121  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
122  {
124  joint_model_map_[joint_model_vector_[i]->getName()] = joint_model_vector_[i];
125  unsigned int vc = joint_model_vector_[i]->getVariableCount();
126  if (vc > 0)
127  {
128  if (vc > 1)
129  is_single_dof_ = false;
130  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
131  if (joint_model_vector_[i]->getMimic() == NULL)
132  {
133  active_joint_model_vector_.push_back(joint_model_vector_[i]);
134  active_joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
136  active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
137  }
138  else
139  mimic_joints_.push_back(joint_model_vector_[i]);
140  for (std::size_t j = 0; j < name_order.size(); ++j)
141  {
142  variable_names_.push_back(name_order[j]);
143  variable_names_set_.insert(name_order[j]);
144  }
145 
146  int first_index = joint_model_vector_[i]->getFirstVariableIndex();
147  for (std::size_t j = 0; j < name_order.size(); ++j)
148  {
149  variable_index_list_.push_back(first_index + j);
150  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
151  }
152  joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
153 
154  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
155  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
156  continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
157 
158  variable_count_ += vc;
159  }
160  else
161  fixed_joints_.push_back(joint_model_vector_[i]);
162  }
163 
164  // now we need to find all the set of joints within this group
165  // that root distinct subtrees
166  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
167  {
168  // if we find that an ancestor is also in the group, then the joint is not a root
169  if (!includesParent(active_joint_model_vector_[i], this))
171  }
172 
173  // when updating this group within a state, it is useful to know
174  // if the full state of a group is contiguous within the full state of the robot
175  if (variable_index_list_.empty())
177  else
178  for (std::size_t i = 1; i < variable_index_list_.size(); ++i)
179  if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
180  {
182  break;
183  }
184 
185  // when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
186  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
187  // if the joint we mimic is also in this group, we will need to do updates when sampling
188  if (hasJointModel(mimic_joints_[i]->getMimic()->getName()))
189  {
190  int src = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
191  int dest = joint_variables_index_map_[mimic_joints_[i]->getName()];
192  GroupMimicUpdate mu(src, dest, mimic_joints_[i]->getMimicFactor(), mimic_joints_[i]->getMimicOffset());
193  group_mimic_update_.push_back(mu);
194  }
195 
196  // now we need to make another pass for group links (we include the fixed joints here)
197  std::set<const LinkModel*> group_links_set;
198  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
199  group_links_set.insert(joint_model_vector_[i]->getChildLinkModel());
200  for (std::set<const LinkModel*>::iterator it = group_links_set.begin(); it != group_links_set.end(); ++it)
201  link_model_vector_.push_back(*it);
202  std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
203 
204  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
205  {
206  link_model_map_[link_model_vector_[i]->getName()] = link_model_vector_[i];
207  link_model_name_vector_.push_back(link_model_vector_[i]->getName());
208  if (!link_model_vector_[i]->getShapes().empty())
209  {
210  link_model_with_geometry_vector_.push_back(link_model_vector_[i]);
211  link_model_with_geometry_name_vector_.push_back(link_model_vector_[i]->getName());
212  }
213  }
214 
215  // compute the common root of this group
216  if (!joint_roots_.empty())
217  {
219  for (std::size_t i = 1; i < joint_roots_.size(); ++i)
221  }
222 
223  // compute updated links
224  for (std::size_t i = 0; i < joint_roots_.size(); ++i)
225  {
226  const std::vector<const LinkModel*>& links = joint_roots_[i]->getDescendantLinkModels();
227  updated_link_model_set_.insert(links.begin(), links.end());
228  }
229  for (std::set<const LinkModel*>::iterator it = updated_link_model_set_.begin(); it != updated_link_model_set_.end();
230  ++it)
231  {
232  updated_link_model_name_set_.insert((*it)->getName());
233  updated_link_model_vector_.push_back(*it);
234  if (!(*it)->getShapes().empty())
235  {
238  updated_link_model_with_geometry_name_set_.insert((*it)->getName());
239  }
240  }
241  std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
243  OrderLinksByIndex());
244  for (std::size_t i = 0; i < updated_link_model_vector_.size(); ++i)
246  for (std::size_t i = 0; i < updated_link_model_with_geometry_vector_.size(); ++i)
248 
249  // check if this group should actually be a chain
250  if (joint_roots_.size() == 1 && active_joint_model_vector_.size() > 1)
251  {
252  bool chain = true;
253  // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
254  // we should always get to the parent.
255  for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
256  if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
257  {
258  chain = false;
259  break;
260  }
261  if (chain)
262  is_chain_ = true;
263  }
264 }
265 
267 {
268 }
269 
270 void moveit::core::JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
271 {
272  subgroup_names_ = subgroups;
273  subgroup_names_set_.clear();
274  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
276 }
277 
278 void moveit::core::JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
279 {
280  sub_groups.resize(subgroup_names_.size());
281  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
282  sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
283 }
284 
285 bool moveit::core::JointModelGroup::hasJointModel(const std::string& joint) const
286 {
287  return joint_model_map_.find(joint) != joint_model_map_.end();
288 }
289 
290 bool moveit::core::JointModelGroup::hasLinkModel(const std::string& link) const
291 {
292  return link_model_map_.find(link) != link_model_map_.end();
293 }
294 
296 {
297  LinkModelMapConst::const_iterator it = link_model_map_.find(name);
298  if (it == link_model_map_.end())
299  {
300  logError("Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
301  return NULL;
302  }
303  return it->second;
304 }
305 
307 {
308  JointModelMapConst::const_iterator it = joint_model_map_.find(name);
309  if (it == joint_model_map_.end())
310  {
311  logError("Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
312  return NULL;
313  }
314  return it->second;
315 }
316 
318  double* values,
319  const JointBoundsVector& active_joint_bounds) const
320 {
321  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
322  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
324  *active_joint_bounds[i]);
325 
326  updateMimicJoints(values);
327 }
328 
330  double* values,
331  const JointBoundsVector& active_joint_bounds,
332  const double* near, double distance) const
333 {
334  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
335  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
337  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
338  near + active_joint_model_start_index_[i], distance);
339  updateMimicJoints(values);
340 }
341 
343  random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds,
344  const double* near, const std::map<JointModel::JointType, double>& distance_map) const
345 {
346  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
347  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
348  {
349  double distance = 0.0;
350  std::map<moveit::core::JointModel::JointType, double>::const_iterator iter =
351  distance_map.find(active_joint_model_vector_[i]->getType());
352  if (iter != distance_map.end())
353  distance = iter->second;
354  else
355  logWarn("Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str());
356  active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(
357  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
358  near + active_joint_model_start_index_[i], distance);
359  }
360  updateMimicJoints(values);
361 }
362 
364  double* values,
365  const JointBoundsVector& active_joint_bounds,
366  const double* near,
367  const std::vector<double>& distances) const
368 {
369  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
370  if (distances.size() != active_joint_model_vector_.size())
371  throw Exception("When sampling random values nearby for group '" + name_ +
372  "', distances vector should be of size " +
373  boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
374  boost::lexical_cast<std::string>(distances.size()));
375  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
377  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
378  near + active_joint_model_start_index_[i], distances[i]);
379  updateMimicJoints(values);
380 }
381 
383  const JointBoundsVector& active_joint_bounds,
384  double margin) const
385 {
386  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
387  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
389  *active_joint_bounds[i], margin))
390  return false;
391  return true;
392 }
393 
395  const JointBoundsVector& active_joint_bounds) const
396 {
397  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
398  bool change = false;
399  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
401  *active_joint_bounds[i]))
402  change = true;
403  if (change)
404  updateMimicJoints(state);
405  return change;
406 }
407 
409 {
410  double max_distance = 0.0;
411  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
412  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
413  active_joint_model_vector_[j]->getDistanceFactor();
414  return max_distance;
415 }
416 
417 double moveit::core::JointModelGroup::distance(const double* state1, const double* state2) const
418 {
419  double d = 0.0;
420  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
421  d += active_joint_model_vector_[i]->getDistanceFactor() *
423  state2 + active_joint_model_start_index_[i]);
424  return d;
425 }
426 
427 void moveit::core::JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
428 {
429  // we interpolate values only for active joint models (non-mimic)
430  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
434 
435  // now we update mimic as needed
436  updateMimicJoints(state);
437 }
438 
440 {
441  // update mimic (only local joints as we are dealing with a local group state)
442  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
443  values[group_mimic_update_[i].dest] =
444  values[group_mimic_update_[i].src] * group_mimic_update_[i].factor + group_mimic_update_[i].offset;
445 }
446 
448  const std::map<std::string, double>& default_state)
449 {
450  default_states_[name] = default_state;
451  default_states_names_.push_back(name);
452 }
453 
455  std::map<std::string, double>& values) const
456 {
457  std::map<std::string, std::map<std::string, double> >::const_iterator it = default_states_.find(name);
458  if (it == default_states_.end())
459  return false;
460  values = it->second;
461  return true;
462 }
463 
465 {
466  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
468  updateMimicJoints(values);
469 }
470 
471 void moveit::core::JointModelGroup::getVariableDefaultPositions(std::map<std::string, double>& values) const
472 {
473  std::vector<double> tmp(variable_count_);
475  for (std::size_t i = 0; i < variable_names_.size(); ++i)
476  values[variable_names_[i]] = tmp[i];
477 }
478 
480 {
481  end_effector_name_ = name;
482 }
483 
484 void moveit::core::JointModelGroup::setEndEffectorParent(const std::string& group, const std::string& link)
485 {
486  end_effector_parent_.first = group;
487  end_effector_parent_.second = link;
488 }
489 
490 void moveit::core::JointModelGroup::attachEndEffector(const std::string& eef_name)
491 {
492  attached_end_effector_names_.push_back(eef_name);
493 }
494 
495 bool moveit::core::JointModelGroup::getEndEffectorTips(std::vector<std::string>& tips) const
496 {
497  // Get a vector of tip links
498  std::vector<const LinkModel*> tip_links;
499  if (!getEndEffectorTips(tip_links))
500  return false;
501 
502  // Convert to string names
503  tips.clear();
504  for (std::size_t i = 0; i < tip_links.size(); ++i)
505  {
506  tips.push_back(tip_links[i]->getName());
507  }
508  return true;
509 }
510 
511 bool moveit::core::JointModelGroup::getEndEffectorTips(std::vector<const LinkModel*>& tips) const
512 {
513  for (std::size_t i = 0; i < getAttachedEndEffectorNames().size(); ++i)
514  {
516  if (!eef)
517  {
518  logError("Unable to find joint model group for eef");
519  return false;
520  }
521  const std::string& eef_parent = eef->getEndEffectorParentGroup().second;
522 
523  const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent);
524  if (!eef_link)
525  {
526  logError("Unable to find end effector link for eef");
527  return false;
528  }
529 
530  tips.push_back(eef_link);
531  }
532  return true;
533 }
534 
536 {
537  std::vector<const moveit::core::LinkModel*> tips;
538  getEndEffectorTips(tips);
539  if (tips.size() == 1)
540  return tips.front();
541  else if (tips.size() > 1)
542  logError("More than one end effector tip found for joint model group, so cannot return only one");
543  else
544  logError("No end effector tips found in joint model group");
545  return NULL;
546 }
547 
548 int moveit::core::JointModelGroup::getVariableGroupIndex(const std::string& variable) const
549 {
550  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
551  if (it == joint_variables_index_map_.end())
552  {
553  logError("Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str());
554  return -1;
555  }
556  return it->second;
557 }
558 
560 {
561  group_kinematics_.first.default_ik_timeout_ = ik_timeout;
562  if (group_kinematics_.first.solver_instance_)
563  group_kinematics_.first.solver_instance_->setDefaultTimeout(ik_timeout);
564  for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
565  it->second.default_ik_timeout_ = ik_timeout;
566 }
567 
569 {
570  group_kinematics_.first.default_ik_attempts_ = ik_attempts;
571  for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
572  it->second.default_ik_attempts_ = ik_attempts;
573 }
574 
575 bool moveit::core::JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
576  std::vector<unsigned int>& joint_bijection) const
577 {
578  joint_bijection.clear();
579  for (std::size_t i = 0; i < ik_jnames.size(); ++i)
580  {
581  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jnames[i]);
582  if (it == joint_variables_index_map_.end())
583  {
584  // skip reported fixed joints
585  if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED)
586  continue;
587  logError("IK solver computes joint values for joint '%s' but group '%s' does not contain such a joint.",
588  ik_jnames[i].c_str(), getName().c_str());
589  return false;
590  }
591  const JointModel* jm = getJointModel(ik_jnames[i]);
592  for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
593  joint_bijection.push_back(it->second + k);
594  }
595  return true;
596 }
597 
599  const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
600 {
601  if (solvers.first)
602  {
603  group_kinematics_.first.allocator_ = solvers.first;
604  group_kinematics_.first.solver_instance_ = solvers.first(this);
605  if (group_kinematics_.first.solver_instance_)
606  {
607  group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
608  group_kinematics_.first.solver_instance_const_ = group_kinematics_.first.solver_instance_;
609  if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
610  group_kinematics_.first.bijection_))
611  group_kinematics_.first.reset();
612  }
613  }
614  else
615  // we now compute a joint bijection only if we have a solver map
616  for (SolverAllocatorMapFn::const_iterator it = solvers.second.begin(); it != solvers.second.end(); ++it)
617  if (it->first->getSolverInstance())
618  {
619  KinematicsSolver& ks = group_kinematics_.second[it->first];
620  ks.allocator_ = it->second;
621  ks.solver_instance_ = const_cast<JointModelGroup*>(it->first)->getSolverInstance();
623  ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
624  ks.default_ik_attempts_ = group_kinematics_.first.default_ik_attempts_;
625  if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
626  {
627  group_kinematics_.second.clear();
628  break;
629  }
630  }
631 }
632 
633 bool moveit::core::JointModelGroup::canSetStateFromIK(const std::string& tip) const
634 {
635  const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
636  if (!solver || tip.empty())
637  return false;
638 
639  const std::vector<std::string>& tip_frames = solver->getTipFrames();
640 
641  if (tip_frames.empty())
642  {
643  logDebug("Group %s has no tip frame(s)", name_.c_str());
644  return false;
645  }
646 
647  // loop through all tip frames supported by the JMG
648  for (std::size_t i = 0; i < tip_frames.size(); ++i)
649  {
650  // remove frame reference, if specified
651  const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
652  const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i];
653  logDebug("joint_model_group.canSetStateFromIK: comparing input tip: %s to this groups tip: %s ", tip_local.c_str(),
654  tip_frame_local.c_str());
655 
656  // Check if the IK solver's tip is the same as the frame of inquiry
657  if (tip_local != tip_frame_local)
658  {
659  // If not the same, check if this planning group includes the frame of inquiry
660  if (hasLinkModel(tip_frame_local))
661  {
662  const LinkModel* lm = getLinkModel(tip_frame_local);
663  const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
664  // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
665  for (LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
666  {
667  if (it->first->getName() == tip_local)
668  return true;
669  }
670  }
671  }
672  else
673  return true;
674  }
675 
676  // Did not find any valid tip frame links to use
677  return false;
678 }
679 
681 {
682  out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
683  out << " * Joints:" << std::endl;
684  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
685  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
686  << std::endl;
687  out << " * Variables:" << std::endl;
688  for (std::size_t i = 0; i < variable_names_.size(); ++i)
689  {
690  int local_idx = joint_variables_index_map_.find(variable_names_[i])->second;
692  out << " '" << variable_names_[i] << "', index "
693  << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_names_[i])) << " in full state, index "
694  << local_idx << " in group state";
695  if (jm->getMimic())
696  out << ", mimic '" << jm->getMimic()->getName() << "'";
697  out << std::endl;
698  out << " " << parent_model_->getVariableBounds(variable_names_[i]) << std::endl;
699  }
700  out << " * Variables Index List:" << std::endl;
701  out << " ";
702  for (std::size_t i = 0; i < variable_index_list_.size(); ++i)
703  out << variable_index_list_[i] << " ";
705  out << "(contiguous)";
706  else
707  out << "(non-contiguous)";
708  out << std::endl;
709  if (group_kinematics_.first)
710  {
711  out << " * Kinematics solver bijection:" << std::endl;
712  out << " ";
713  for (std::size_t i = 0; i < group_kinematics_.first.bijection_.size(); ++i)
714  out << group_kinematics_.first.bijection_[i] << " ";
715  out << std::endl;
716  }
717  if (!group_kinematics_.second.empty())
718  {
719  out << " * Compound kinematics solver:" << std::endl;
720  for (KinematicsSolverMap::const_iterator it = group_kinematics_.second.begin();
721  it != group_kinematics_.second.end(); ++it)
722  {
723  out << " " << it->first->getName() << ":";
724  for (std::size_t i = 0; i < it->second.bijection_.size(); ++i)
725  out << " " << it->second.bijection_[i];
726  out << std::endl;
727  }
728  }
729 
730  if (!group_mimic_update_.empty())
731  {
732  out << " * Local Mimic Updates:" << std::endl;
733  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
734  out << " [" << group_mimic_update_[i].dest << "] = " << group_mimic_update_[i].factor << " * ["
735  << group_mimic_update_[i].src << "] + " << group_mimic_update_[i].offset << std::endl;
736  }
737  out << std::endl;
738 }
d
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints. This only updates mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, there are no values to be read (values is only the group state)
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
const kinematics::KinematicsBaseConstPtr & getSolverInstance() const
std::set< const LinkModel * > updated_link_model_with_geometry_set_
The list of downstream link models in the order they should be updated (may include links that are no...
void setEndEffectorParent(const std::string &group, const std::string &link)
If this group is an end-effector, specify the parent group (e.g., the arm holding the eef) and the li...
std::vector< GroupMimicUpdate > group_mimic_update_
std::vector< std::string > subgroup_names_
The set of labelled subgroups that are included in this group.
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:200
std::set< const LinkModel * > updated_link_model_set_
The list of downstream link models in the order they should be updated (may include links that are no...
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
bool getVariableDefaultPositions(const std::string &name, std::map< std::string, double > &values) const
Get the values that correspond to a named state as read from the URDF. Return false on failure...
std::map< std::string, std::map< std::string, double > > default_states_
The set of default states specified for this group in the SRDF.
unsigned int variable_count_
The number of variables necessary to describe this group of joints.
std::set< std::string > variable_names_set_
The names of the DOF that make up this group (this is just a sequence of joint variable names; not ne...
const std::string & getName() const
Get the name of the joint group.
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
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:395
std::set< std::string > updated_link_model_name_set_
The list of downstream link names in the order they should be updated (may include links that are not...
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const double distance) const
Compute random values for the state of the joint group.
const std::pair< std::string, std::string > & getEndEffectorParentGroup() const
Get the name of the group this end-effector attaches to (first) and the name of the link in that grou...
bool canSetStateFromIK(const std::string &tip) const
bool getEndEffectorTips(std::vector< const LinkModel * > &tips) const
Get a vector of end effector tips included in a particular joint model group as defined by the SRDF e...
int getLocalVariableIndex(const std::string &variable) const
Get the index of the variable within this joint.
Definition: joint_model.cpp:83
const moveit::core::LinkModel * getOnlyOneEndEffectorTip() const
Get one end effector tip, throwing an error if there ends up being more in the joint model group This...
std::vector< const JointModel * > joint_roots_
The list of active joint models that are roots in this group.
std::string name_
Name of group.
std::vector< std::string > joint_model_name_vector_
Names of joints in the order they appear in the group state.
bool is_contiguous_index_list_
True if the state of this group is contiguous within the full robot state; this also means that the i...
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:68
std::vector< int > variable_index_list_
The list of index values this group includes, with respect to a full robot state; this includes mimic...
std::vector< std::string > link_model_with_geometry_name_vector_
The names of the links in this group that also have geometry.
std::vector< const LinkModel * > updated_link_model_vector_
The list of downstream link models in the order they should be updated (may include links that are no...
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
std::vector< std::string > default_states_names_
The names of the default states specified for this group in the SRDF.
void setDefaultIKTimeout(double ik_timeout)
Set the default IK timeout.
std::vector< const LinkModel * > link_model_with_geometry_vector_
void interpolate(const double *from, const double *to, double t, double *state) const
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group. ...
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
const LinkModel * getLinkModel(const std::string &link) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
std::vector< const JointModel * > mimic_joints_
Joints that mimic other joints.
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
std::vector< std::string > link_model_name_vector_
The names of the links in this group.
std::vector< std::string > updated_link_model_with_geometry_name_vector_
The list of downstream link names in the order they should be updated (may include links that are not...
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel * > &joint_vector, const RobotModel *parent_model)
const RobotModel * parent_model_
Owner model.
std::vector< const JointModel::Bounds * > JointBoundsVector
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
VariableIndexMap joint_variables_index_map_
The group includes all the joint variables that make up the joints the group consists of...
std::pair< KinematicsSolver, KinematicsSolverMap > group_kinematics_
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
void attachEndEffector(const std::string &eef_name)
Notify this group that there is an end-effector attached to it.
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
std::vector< const JointModel * > fixed_joints_
The joints that have no DOF (fixed)
kinematics::KinematicsBaseConstPtr solver_instance_const_
std::vector< unsigned int > bijection_
The mapping between the order of the joints in the group and the order of the joints in the kinematic...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
std::vector< std::string > attached_end_effector_names_
If an end-effector is attached to this group, the name of that end-effector is stored in this variabl...
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
bool enforcePositionBounds(double *state) const
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
LinkModelMapConst link_model_map_
A map from link names to their instances.
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< unsigned int > &joint_bijection) const
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this group contains.
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
std::vector< std::string > variable_names_
The names of the DOF that make up this group (this is just a sequence of joint variable names; not ne...
std::vector< int > active_joint_model_start_index_
For each active joint model in this group, hold the index at which the corresponding joint state star...
std::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
const JointModel * getCommonRoot(const JointModel *a, const JointModel *b) const
Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument...
Definition: robot_model.h:413
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Definition: link_model.h:196
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group) ...
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
double distance(const double *state1, const double *state2) const
std::pair< std::string, std::string > end_effector_parent_
First: name of the group that is parent to this end-effector group; Second: the link this in the pare...
std::vector< std::string > updated_link_model_name_vector_
The list of downstream link names in the order they should be updated (may include links that are not...
Main namespace for MoveIt!
std::vector< const LinkModel * > updated_link_model_with_geometry_vector_
The list of downstream link models in the order they should be updated (may include links that are no...
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:69
void setDefaultIKAttempts(unsigned int ik_attempts)
Set the default IK attempts.
JointType getType() const
Get the type of joint.
Definition: joint_model.h:137
std::set< std::string > updated_link_model_with_geometry_name_set_
The list of downstream link names in the order they should be updated (may include links that are not...
std::vector< const LinkModel * > link_model_vector_
The links that are on the direct lineage between joints and joint_roots_, as well as the children of ...
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:362


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jan 21 2018 03:54:29