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 JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Model::Group& config,
101  const std::vector<const JointModel*>& unsorted_group_joints,
102  const RobotModel* parent_model)
103  : parent_model_(parent_model)
104  , name_(group_name)
105  , common_root_(NULL)
106  , variable_count_(0)
107  , is_contiguous_index_list_(true)
108  , is_chain_(false)
109  , is_single_dof_(true)
110  , config_(config)
111 {
112  // sort joints in Depth-First order
113  joint_model_vector_ = unsorted_group_joints;
114  std::sort(joint_model_vector_.begin(), joint_model_vector_.end(), OrderJointsByIndex());
116 
117  // figure out active joints, mimic joints, fixed joints
118  // construct index maps, list of variables
119  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
120  {
122  joint_model_map_[joint_model_vector_[i]->getName()] = joint_model_vector_[i];
123  unsigned int vc = joint_model_vector_[i]->getVariableCount();
124  if (vc > 0)
125  {
126  if (vc > 1)
127  is_single_dof_ = false;
128  const std::vector<std::string>& name_order = joint_model_vector_[i]->getVariableNames();
129  if (joint_model_vector_[i]->getMimic() == NULL)
130  {
131  active_joint_model_vector_.push_back(joint_model_vector_[i]);
132  active_joint_model_name_vector_.push_back(joint_model_vector_[i]->getName());
134  active_joint_models_bounds_.push_back(&joint_model_vector_[i]->getVariableBounds());
135  }
136  else
137  mimic_joints_.push_back(joint_model_vector_[i]);
138  for (std::size_t j = 0; j < name_order.size(); ++j)
139  {
140  variable_names_.push_back(name_order[j]);
141  variable_names_set_.insert(name_order[j]);
142  }
143 
144  int first_index = joint_model_vector_[i]->getFirstVariableIndex();
145  for (std::size_t j = 0; j < name_order.size(); ++j)
146  {
147  variable_index_list_.push_back(first_index + j);
148  joint_variables_index_map_[name_order[j]] = variable_count_ + j;
149  }
150  joint_variables_index_map_[joint_model_vector_[i]->getName()] = variable_count_;
151 
152  if (joint_model_vector_[i]->getType() == JointModel::REVOLUTE &&
153  static_cast<const RevoluteJointModel*>(joint_model_vector_[i])->isContinuous())
154  continuous_joint_model_vector_.push_back(joint_model_vector_[i]);
155 
156  variable_count_ += vc;
157  }
158  else
159  fixed_joints_.push_back(joint_model_vector_[i]);
160  }
161 
162  // now we need to find all the set of joints within this group
163  // that root distinct subtrees
164  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
165  {
166  // if we find that an ancestor is also in the group, then the joint is not a root
167  if (!includesParent(active_joint_model_vector_[i], this))
169  }
170 
171  // when updating this group within a state, it is useful to know
172  // if the full state of a group is contiguous within the full state of the robot
173  if (variable_index_list_.empty())
175  else
176  for (std::size_t i = 1; i < variable_index_list_.size(); ++i)
177  if (variable_index_list_[i] != variable_index_list_[i - 1] + 1)
178  {
180  break;
181  }
182 
183  // when updating/sampling a group state only, only mimic joints that have their parent within the group get updated.
184  for (std::size_t i = 0; i < mimic_joints_.size(); ++i)
185  // if the joint we mimic is also in this group, we will need to do updates when sampling
186  if (hasJointModel(mimic_joints_[i]->getMimic()->getName()))
187  {
188  int src = joint_variables_index_map_[mimic_joints_[i]->getMimic()->getName()];
189  int dest = joint_variables_index_map_[mimic_joints_[i]->getName()];
190  GroupMimicUpdate mu(src, dest, mimic_joints_[i]->getMimicFactor(), mimic_joints_[i]->getMimicOffset());
191  group_mimic_update_.push_back(mu);
192  }
193 
194  // now we need to make another pass for group links (we include the fixed joints here)
195  std::set<const LinkModel*> group_links_set;
196  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
197  group_links_set.insert(joint_model_vector_[i]->getChildLinkModel());
198  for (std::set<const LinkModel*>::iterator it = group_links_set.begin(); it != group_links_set.end(); ++it)
199  link_model_vector_.push_back(*it);
200  std::sort(link_model_vector_.begin(), link_model_vector_.end(), OrderLinksByIndex());
201 
202  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
203  {
204  link_model_map_[link_model_vector_[i]->getName()] = link_model_vector_[i];
205  link_model_name_vector_.push_back(link_model_vector_[i]->getName());
206  if (!link_model_vector_[i]->getShapes().empty())
207  {
208  link_model_with_geometry_vector_.push_back(link_model_vector_[i]);
209  link_model_with_geometry_name_vector_.push_back(link_model_vector_[i]->getName());
210  }
211  }
212 
213  // compute the common root of this group
214  if (!joint_roots_.empty())
215  {
217  for (std::size_t i = 1; i < joint_roots_.size(); ++i)
219  }
220 
221  // compute updated links
222  for (std::size_t i = 0; i < joint_roots_.size(); ++i)
223  {
224  const std::vector<const LinkModel*>& links = joint_roots_[i]->getDescendantLinkModels();
225  updated_link_model_set_.insert(links.begin(), links.end());
226  }
227  for (std::set<const LinkModel*>::iterator it = updated_link_model_set_.begin(); it != updated_link_model_set_.end();
228  ++it)
229  {
230  updated_link_model_name_set_.insert((*it)->getName());
231  updated_link_model_vector_.push_back(*it);
232  if (!(*it)->getShapes().empty())
233  {
236  updated_link_model_with_geometry_name_set_.insert((*it)->getName());
237  }
238  }
239  std::sort(updated_link_model_vector_.begin(), updated_link_model_vector_.end(), OrderLinksByIndex());
241  OrderLinksByIndex());
242  for (std::size_t i = 0; i < updated_link_model_vector_.size(); ++i)
244  for (std::size_t i = 0; i < updated_link_model_with_geometry_vector_.size(); ++i)
246 
247  // check if this group should actually be a chain
248  if (joint_roots_.size() == 1 && active_joint_model_vector_.size() > 1)
249  {
250  bool chain = true;
251  // due to our sorting, the joints are sorted in a DF fashion, so looking at them in reverse,
252  // we should always get to the parent.
253  for (std::size_t k = joint_model_vector_.size() - 1; k > 0; --k)
254  if (!jointPrecedes(joint_model_vector_[k], joint_model_vector_[k - 1]))
255  {
256  chain = false;
257  break;
258  }
259  if (chain)
260  is_chain_ = true;
261  }
262 }
263 
265 {
266 }
267 
268 void JointModelGroup::setSubgroupNames(const std::vector<std::string>& subgroups)
269 {
270  subgroup_names_ = subgroups;
271  subgroup_names_set_.clear();
272  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
274 }
275 
276 void JointModelGroup::getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const
277 {
278  sub_groups.resize(subgroup_names_.size());
279  for (std::size_t i = 0; i < subgroup_names_.size(); ++i)
280  sub_groups[i] = parent_model_->getJointModelGroup(subgroup_names_[i]);
281 }
282 
283 bool JointModelGroup::hasJointModel(const std::string& joint) const
284 {
285  return joint_model_map_.find(joint) != joint_model_map_.end();
286 }
287 
288 bool JointModelGroup::hasLinkModel(const std::string& link) const
289 {
290  return link_model_map_.find(link) != link_model_map_.end();
291 }
292 
293 const LinkModel* JointModelGroup::getLinkModel(const std::string& name) const
294 {
295  LinkModelMapConst::const_iterator it = link_model_map_.find(name);
296  if (it == link_model_map_.end())
297  {
298  CONSOLE_BRIDGE_logError("Link '%s' not found in group '%s'", name.c_str(), name_.c_str());
299  return NULL;
300  }
301  return it->second;
302 }
303 
304 const JointModel* JointModelGroup::getJointModel(const std::string& name) const
305 {
306  JointModelMapConst::const_iterator it = joint_model_map_.find(name);
307  if (it == joint_model_map_.end())
308  {
309  CONSOLE_BRIDGE_logError("Joint '%s' not found in group '%s'", name.c_str(), name_.c_str());
310  return NULL;
311  }
312  return it->second;
313 }
314 
316  const JointBoundsVector& active_joint_bounds) const
317 {
318  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
319  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
321  *active_joint_bounds[i]);
322 
323  updateMimicJoints(values);
324 }
325 
327  const JointBoundsVector& active_joint_bounds, const double* near,
328  double distance) const
329 {
330  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
331  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
333  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
334  near + active_joint_model_start_index_[i], distance);
335  updateMimicJoints(values);
336 }
337 
339  random_numbers::RandomNumberGenerator& rng, double* values, const JointBoundsVector& active_joint_bounds,
340  const double* near, const std::map<JointModel::JointType, double>& distance_map) const
341 {
342  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
343  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
344  {
345  double distance = 0.0;
346  std::map<JointModel::JointType, double>::const_iterator iter =
347  distance_map.find(active_joint_model_vector_[i]->getType());
348  if (iter != distance_map.end())
349  distance = iter->second;
350  else
351  CONSOLE_BRIDGE_logWarn("Did not pass in distance for '%s'", active_joint_model_vector_[i]->getName().c_str());
352  active_joint_model_vector_[i]->getVariableRandomPositionsNearBy(
353  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
354  near + active_joint_model_start_index_[i], distance);
355  }
356  updateMimicJoints(values);
357 }
358 
360  const JointBoundsVector& active_joint_bounds, const double* near,
361  const std::vector<double>& distances) const
362 {
363  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
364  if (distances.size() != active_joint_model_vector_.size())
365  throw Exception("When sampling random values nearby for group '" + name_ +
366  "', distances vector should be of size " +
367  boost::lexical_cast<std::string>(active_joint_model_vector_.size()) + ", but it is of size " +
368  boost::lexical_cast<std::string>(distances.size()));
369  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
371  rng, values + active_joint_model_start_index_[i], *active_joint_bounds[i],
372  near + active_joint_model_start_index_[i], distances[i]);
373  updateMimicJoints(values);
374 }
375 
376 bool JointModelGroup::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
377  double margin) const
378 {
379  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
380  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
382  *active_joint_bounds[i], margin))
383  return false;
384  return true;
385 }
386 
387 bool JointModelGroup::enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const
388 {
389  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
390  bool change = false;
391  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
393  *active_joint_bounds[i]))
394  change = true;
395  if (change)
396  updateMimicJoints(state);
397  return change;
398 }
399 
400 double JointModelGroup::getMaximumExtent(const JointBoundsVector& active_joint_bounds) const
401 {
402  double max_distance = 0.0;
403  for (std::size_t j = 0; j < active_joint_model_vector_.size(); ++j)
404  max_distance += active_joint_model_vector_[j]->getMaximumExtent(*active_joint_bounds[j]) *
405  active_joint_model_vector_[j]->getDistanceFactor();
406  return max_distance;
407 }
408 
409 double JointModelGroup::distance(const double* state1, const double* state2) const
410 {
411  double d = 0.0;
412  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
413  d += active_joint_model_vector_[i]->getDistanceFactor() *
415  state2 + active_joint_model_start_index_[i]);
416  return d;
417 }
418 
419 void JointModelGroup::interpolate(const double* from, const double* to, double t, double* state) const
420 {
421  // we interpolate values only for active joint models (non-mimic)
422  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
426 
427  // now we update mimic as needed
428  updateMimicJoints(state);
429 }
430 
431 void JointModelGroup::updateMimicJoints(double* values) const
432 {
433  // update mimic (only local joints as we are dealing with a local group state)
434  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
435  values[group_mimic_update_[i].dest] =
436  values[group_mimic_update_[i].src] * group_mimic_update_[i].factor + group_mimic_update_[i].offset;
437 }
438 
439 void JointModelGroup::addDefaultState(const std::string& name, const std::map<std::string, double>& default_state)
440 {
441  default_states_[name] = default_state;
442  default_states_names_.push_back(name);
443 }
444 
445 bool JointModelGroup::getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const
446 {
447  std::map<std::string, std::map<std::string, double> >::const_iterator it = default_states_.find(name);
448  if (it == default_states_.end())
449  return false;
450  values = it->second;
451  return true;
452 }
453 
455 {
456  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
458  updateMimicJoints(values);
459 }
460 
461 void JointModelGroup::getVariableDefaultPositions(std::map<std::string, double>& values) const
462 {
463  std::vector<double> tmp(variable_count_);
465  for (std::size_t i = 0; i < variable_names_.size(); ++i)
466  values[variable_names_[i]] = tmp[i];
467 }
468 
469 void JointModelGroup::setEndEffectorName(const std::string& name)
470 {
471  end_effector_name_ = name;
472 }
473 
474 void JointModelGroup::setEndEffectorParent(const std::string& group, const std::string& link)
475 {
476  end_effector_parent_.first = group;
477  end_effector_parent_.second = link;
478 }
479 
480 void JointModelGroup::attachEndEffector(const std::string& eef_name)
481 {
482  attached_end_effector_names_.push_back(eef_name);
483 }
484 
485 bool JointModelGroup::getEndEffectorTips(std::vector<std::string>& tips) const
486 {
487  // Get a vector of tip links
488  std::vector<const LinkModel*> tip_links;
489  if (!getEndEffectorTips(tip_links))
490  return false;
491 
492  // Convert to string names
493  tips.clear();
494  for (std::size_t i = 0; i < tip_links.size(); ++i)
495  {
496  tips.push_back(tip_links[i]->getName());
497  }
498  return true;
499 }
500 
501 bool JointModelGroup::getEndEffectorTips(std::vector<const LinkModel*>& tips) const
502 {
503  for (std::size_t i = 0; i < getAttachedEndEffectorNames().size(); ++i)
504  {
506  if (!eef)
507  {
508  CONSOLE_BRIDGE_logError("Unable to find joint model group for eef");
509  return false;
510  }
511  const std::string& eef_parent = eef->getEndEffectorParentGroup().second;
512 
513  const LinkModel* eef_link = parent_model_->getLinkModel(eef_parent);
514  if (!eef_link)
515  {
516  CONSOLE_BRIDGE_logError("Unable to find end effector link for eef");
517  return false;
518  }
519 
520  tips.push_back(eef_link);
521  }
522  return true;
523 }
524 
526 {
527  std::vector<const LinkModel*> tips;
528  getEndEffectorTips(tips);
529  if (tips.size() == 1)
530  return tips.front();
531  else if (tips.size() > 1)
532  CONSOLE_BRIDGE_logError("More than one end effector tip found for joint model group, so cannot return only one");
533  else
534  CONSOLE_BRIDGE_logError("No end effector tips found in joint model group");
535  return NULL;
536 }
537 
538 int JointModelGroup::getVariableGroupIndex(const std::string& variable) const
539 {
540  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(variable);
541  if (it == joint_variables_index_map_.end())
542  {
543  CONSOLE_BRIDGE_logError("Variable '%s' is not part of group '%s'", variable.c_str(), name_.c_str());
544  return -1;
545  }
546  return it->second;
547 }
548 
550 {
551  group_kinematics_.first.default_ik_timeout_ = ik_timeout;
552  if (group_kinematics_.first.solver_instance_)
553  group_kinematics_.first.solver_instance_->setDefaultTimeout(ik_timeout);
554  for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
555  it->second.default_ik_timeout_ = ik_timeout;
556 }
557 
558 void JointModelGroup::setDefaultIKAttempts(unsigned int ik_attempts)
559 {
560  group_kinematics_.first.default_ik_attempts_ = ik_attempts;
561  for (KinematicsSolverMap::iterator it = group_kinematics_.second.begin(); it != group_kinematics_.second.end(); ++it)
562  it->second.default_ik_attempts_ = ik_attempts;
563 }
564 
565 bool JointModelGroup::computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
566  std::vector<unsigned int>& joint_bijection) const
567 {
568  joint_bijection.clear();
569  for (std::size_t i = 0; i < ik_jnames.size(); ++i)
570  {
571  VariableIndexMap::const_iterator it = joint_variables_index_map_.find(ik_jnames[i]);
572  if (it == joint_variables_index_map_.end())
573  {
574  // skip reported fixed joints
575  if (hasJointModel(ik_jnames[i]) && getJointModel(ik_jnames[i])->getType() == JointModel::FIXED)
576  continue;
577  CONSOLE_BRIDGE_logError("IK solver computes joint values for joint '%s' "
578  "but group '%s' does not contain such a joint.",
579  ik_jnames[i].c_str(), getName().c_str());
580  return false;
581  }
582  const JointModel* jm = getJointModel(ik_jnames[i]);
583  for (unsigned int k = 0; k < jm->getVariableCount(); ++k)
584  joint_bijection.push_back(it->second + k);
585  }
586  return true;
587 }
588 
589 void JointModelGroup::setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers)
590 {
591  if (solvers.first)
592  {
593  group_kinematics_.first.allocator_ = solvers.first;
594  group_kinematics_.first.solver_instance_ = solvers.first(this);
595  if (group_kinematics_.first.solver_instance_)
596  {
597  group_kinematics_.first.solver_instance_->setDefaultTimeout(group_kinematics_.first.default_ik_timeout_);
598  group_kinematics_.first.solver_instance_const_ = group_kinematics_.first.solver_instance_;
599  if (!computeIKIndexBijection(group_kinematics_.first.solver_instance_->getJointNames(),
600  group_kinematics_.first.bijection_))
601  group_kinematics_.first.reset();
602  }
603  }
604  else
605  // we now compute a joint bijection only if we have a solver map
606  for (SolverAllocatorMapFn::const_iterator it = solvers.second.begin(); it != solvers.second.end(); ++it)
607  if (it->first->getSolverInstance())
608  {
609  KinematicsSolver& ks = group_kinematics_.second[it->first];
610  ks.allocator_ = it->second;
611  ks.solver_instance_ = const_cast<JointModelGroup*>(it->first)->getSolverInstance();
613  ks.default_ik_timeout_ = group_kinematics_.first.default_ik_timeout_;
614  ks.default_ik_attempts_ = group_kinematics_.first.default_ik_attempts_;
615  if (!computeIKIndexBijection(ks.solver_instance_->getJointNames(), ks.bijection_))
616  {
617  group_kinematics_.second.clear();
618  break;
619  }
620  }
621 }
622 
623 bool JointModelGroup::canSetStateFromIK(const std::string& tip) const
624 {
625  const kinematics::KinematicsBaseConstPtr& solver = getSolverInstance();
626  if (!solver || tip.empty())
627  return false;
628 
629  const std::vector<std::string>& tip_frames = solver->getTipFrames();
630 
631  if (tip_frames.empty())
632  {
633  CONSOLE_BRIDGE_logDebug("Group %s has no tip frame(s)", name_.c_str());
634  return false;
635  }
636 
637  // loop through all tip frames supported by the JMG
638  for (std::size_t i = 0; i < tip_frames.size(); ++i)
639  {
640  // remove frame reference, if specified
641  const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
642  const std::string& tip_frame_local = tip_frames[i][0] == '/' ? tip_frames[i].substr(1) : tip_frames[i];
643  CONSOLE_BRIDGE_logDebug("joint_model_group.canSetStateFromIK: comparing input tip: %s to this groups tip: %s ",
644  tip_local.c_str(), tip_frame_local.c_str());
645 
646  // Check if the IK solver's tip is the same as the frame of inquiry
647  if (tip_local != tip_frame_local)
648  {
649  // If not the same, check if this planning group includes the frame of inquiry
650  if (hasLinkModel(tip_frame_local))
651  {
652  const LinkModel* lm = getLinkModel(tip_frame_local);
653  const LinkTransformMap& fixed_links = lm->getAssociatedFixedTransforms();
654  // Check if our frame of inquiry is located anywhere further down the chain (towards the tip of the arm)
655  for (LinkTransformMap::const_iterator it = fixed_links.begin(); it != fixed_links.end(); ++it)
656  {
657  if (it->first->getName() == tip_local)
658  return true;
659  }
660  }
661  }
662  else
663  return true;
664  }
665 
666  // Did not find any valid tip frame links to use
667  return false;
668 }
669 
670 void JointModelGroup::printGroupInfo(std::ostream& out) const
671 {
672  out << "Group '" << name_ << "' using " << variable_count_ << " variables" << std::endl;
673  out << " * Joints:" << std::endl;
674  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
675  out << " '" << joint_model_vector_[i]->getName() << "' (" << joint_model_vector_[i]->getTypeName() << ")"
676  << std::endl;
677  out << " * Variables:" << std::endl;
678  for (std::size_t i = 0; i < variable_names_.size(); ++i)
679  {
680  int local_idx = joint_variables_index_map_.find(variable_names_[i])->second;
682  out << " '" << variable_names_[i] << "', index "
683  << (jm->getFirstVariableIndex() + jm->getLocalVariableIndex(variable_names_[i])) << " in full state, index "
684  << local_idx << " in group state";
685  if (jm->getMimic())
686  out << ", mimic '" << jm->getMimic()->getName() << "'";
687  out << std::endl;
688  out << " " << parent_model_->getVariableBounds(variable_names_[i]) << std::endl;
689  }
690  out << " * Variables Index List:" << std::endl;
691  out << " ";
692  for (std::size_t i = 0; i < variable_index_list_.size(); ++i)
693  out << variable_index_list_[i] << " ";
695  out << "(contiguous)";
696  else
697  out << "(non-contiguous)";
698  out << std::endl;
699  if (group_kinematics_.first)
700  {
701  out << " * Kinematics solver bijection:" << std::endl;
702  out << " ";
703  for (std::size_t i = 0; i < group_kinematics_.first.bijection_.size(); ++i)
704  out << group_kinematics_.first.bijection_[i] << " ";
705  out << std::endl;
706  }
707  if (!group_kinematics_.second.empty())
708  {
709  out << " * Compound kinematics solver:" << std::endl;
710  for (KinematicsSolverMap::const_iterator it = group_kinematics_.second.begin();
711  it != group_kinematics_.second.end(); ++it)
712  {
713  out << " " << it->first->getName() << ":";
714  for (std::size_t i = 0; i < it->second.bijection_.size(); ++i)
715  out << " " << it->second.bijection_[i];
716  out << std::endl;
717  }
718  }
719 
720  if (!group_mimic_update_.empty())
721  {
722  out << " * Local Mimic Updates:" << std::endl;
723  for (std::size_t i = 0; i < group_mimic_update_.size(); ++i)
724  out << " [" << group_mimic_update_[i].dest << "] = " << group_mimic_update_[i].factor << " * ["
725  << group_mimic_update_[i].src << "] + " << group_mimic_update_[i].offset << std::endl;
726  }
727  out << std::endl;
728 }
729 
730 } // end of namespace core
731 } // end of namespace moveit
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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.
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.
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...
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.
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::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.
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
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:408
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.
#define CONSOLE_BRIDGE_logDebug(fmt,...)
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:87
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...
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
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...
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
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.
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.
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< 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...
#define CONSOLE_BRIDGE_logError(fmt,...)
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:426
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 * 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
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...
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 Sat Apr 21 2018 02:54:51