joint_model_group.h
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) 2008-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 Willow Garage, Inc. 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 
38 #ifndef MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_
39 #define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_
40 
44 #include <srdfdom/model.h>
45 #include <boost/function.hpp>
46 #include <set>
47 
48 namespace moveit
49 {
50 namespace core
51 {
52 class RobotModel;
54 
56 typedef boost::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
57 
59 typedef std::map<const JointModelGroup*, SolverAllocatorFn> SolverAllocatorMapFn;
60 
62 typedef std::map<std::string, JointModelGroup*> JointModelGroupMap;
63 
65 typedef std::map<std::string, const JointModelGroup*> JointModelGroupMapConst;
66 
67 typedef std::vector<const JointModel::Bounds*> JointBoundsVector;
68 
70 {
71 public:
73  {
75  {
76  }
77 
79  operator bool() const
80  {
81  return allocator_ && !bijection_.empty() && solver_instance_;
82  }
83 
84  void reset()
85  {
86  solver_instance_.reset();
87  bijection_.clear();
88  }
89 
91  SolverAllocatorFn allocator_;
92 
98  std::vector<unsigned int> bijection_;
99 
100  kinematics::KinematicsBasePtr solver_instance_;
101 
103  };
104 
106  typedef std::map<const JointModelGroup*, KinematicsSolver> KinematicsSolverMap;
107 
108  JointModelGroup(const std::string& name, const srdf::Model::Group& config,
109  const std::vector<const JointModel*>& joint_vector, const RobotModel* parent_model);
110 
112 
114  const RobotModel& getParentModel() const
115  {
116  return *parent_model_;
117  }
118 
120  const std::string& getName() const
121  {
122  return name_;
123  }
124 
127  {
128  return config_;
129  }
130 
132  bool hasJointModel(const std::string& joint) const;
133 
135  bool hasLinkModel(const std::string& link) const;
136 
138  const JointModel* getJointModel(const std::string& joint) const;
139 
141  const LinkModel* getLinkModel(const std::string& link) const;
142 
144  const std::vector<const JointModel*>& getJointModels() const
145  {
146  return joint_model_vector_;
147  }
148 
151  const std::vector<std::string>& getJointModelNames() const
152  {
154  }
155 
157  const std::vector<const JointModel*>& getActiveJointModels() const
158  {
160  }
161 
164  const std::vector<std::string>& getActiveJointModelNames() const
165  {
167  }
168 
170  const std::vector<const JointModel*>& getFixedJointModels() const
171  {
172  return fixed_joints_;
173  }
174 
176  const std::vector<const JointModel*>& getMimicJointModels() const
177  {
178  return mimic_joints_;
179  }
180 
182  const std::vector<const JointModel*>& getContinuousJointModels() const
183  {
185  }
186 
189  const std::vector<std::string>& getVariableNames() const
190  {
191  return variable_names_;
192  }
193 
201  const std::vector<const JointModel*>& getJointRoots() const
202  {
203  return joint_roots_;
204  }
205 
207  const JointModel* getCommonRoot() const
208  {
209  return common_root_;
210  }
211 
213  const std::vector<const LinkModel*>& getLinkModels() const
214  {
215  return link_model_vector_;
216  }
217 
219  const std::vector<std::string>& getLinkModelNames() const
220  {
222  }
223 
225  const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
226  {
228  }
229 
234  const std::vector<const LinkModel*>& getUpdatedLinkModels() const
235  {
237  }
238 
240  const std::set<const LinkModel*>& getUpdatedLinkModelsSet() const
241  {
243  }
244 
246  const std::vector<std::string>& getUpdatedLinkModelNames() const
247  {
249  }
250 
254  const std::vector<const LinkModel*>& getUpdatedLinkModelsWithGeometry() const
255  {
257  }
258 
260  const std::set<const LinkModel*>& getUpdatedLinkModelsWithGeometrySet() const
261  {
263  }
264 
266  const std::vector<std::string>& getUpdatedLinkModelsWithGeometryNames() const
267  {
269  }
270 
272  const std::set<std::string>& getUpdatedLinkModelsWithGeometryNamesSet() const
273  {
275  }
276 
280  bool isLinkUpdated(const std::string& name) const
281  {
283  }
284 
286  const std::vector<int>& getVariableIndexList() const
287  {
288  return variable_index_list_;
289  }
290 
292  int getVariableGroupIndex(const std::string& variable) const;
293 
295  const std::vector<std::string>& getDefaultStateNames() const
296  {
297  return default_states_names_;
298  }
299 
300  void addDefaultState(const std::string& name, const std::map<std::string, double>& default_state);
301 
303  bool getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const;
304 
306  void getVariableDefaultPositions(std::map<std::string, double>& values) const;
307 
309  void getVariableDefaultPositions(std::vector<double>& values) const
310  {
311  values.resize(variable_count_);
312  getVariableDefaultPositions(&values[0]);
313  }
314 
316  void getVariableDefaultPositions(double* values) const;
317 
320  {
322  }
323 
325  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
326  {
327  values.resize(variable_count_);
329  }
330 
332  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near,
333  const double distance) const
334  {
336  }
339  const std::vector<double>& near, double distance) const
340  {
341  values.resize(variable_count_);
342  getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance);
343  }
344 
347  const std::vector<double>& near,
348  const std::map<JointModel::JointType, double>& distance_map) const
349  {
350  values.resize(variable_count_);
351  getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance_map);
352  }
353 
355  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near,
356  const std::vector<double>& distances) const
357  {
358  getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distances);
359  }
362  const std::vector<double>& near, const std::vector<double>& distances) const
363  {
364  values.resize(variable_count_);
365  getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distances);
366  }
367 
369  const JointBoundsVector& active_joint_bounds) const;
370 
373  const JointBoundsVector& active_joint_bounds, const double* near,
374  const double distance) const;
375 
378  const JointBoundsVector& active_joint_bounds, const double* near,
379  const std::map<JointModel::JointType, double>& distance_map) const;
380 
383  const JointBoundsVector& active_joint_bounds, const double* near,
384  const std::vector<double>& distances) const;
385 
386  bool enforcePositionBounds(double* state) const
387  {
389  }
390 
391  bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const;
392  bool satisfiesPositionBounds(const double* state, double margin = 0.0) const
393  {
395  }
396  bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
397  double margin = 0.0) const;
398 
399  double getMaximumExtent() const
400  {
402  }
403  double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
404 
405  double distance(const double* state1, const double* state2) const;
406  void interpolate(const double* from, const double* to, double t, double* state) const;
407 
410  unsigned int getVariableCount() const
411  {
412  return variable_count_;
413  }
414 
417  unsigned int getActiveVariableCount() const
418  {
419  return active_variable_count_;
420  }
421 
423  void setSubgroupNames(const std::vector<std::string>& subgroups);
424 
426  const std::vector<std::string>& getSubgroupNames() const
427  {
428  return subgroup_names_;
429  }
430 
432  void getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const;
433 
435  bool isSubgroup(const std::string& group) const
436  {
437  return subgroup_names_set_.find(group) != subgroup_names_set_.end();
438  }
439 
441  bool isChain() const
442  {
443  return is_chain_;
444  }
445 
447  bool isSingleDOFJoints() const
448  {
449  return is_single_dof_;
450  }
451 
453  bool isEndEffector() const
454  {
455  return !end_effector_name_.empty();
456  }
457 
459  {
461  }
462 
464  const std::string& getEndEffectorName() const
465  {
466  return end_effector_name_;
467  }
468 
470  void setEndEffectorName(const std::string& name);
471 
475  void setEndEffectorParent(const std::string& group, const std::string& link);
476 
478  void attachEndEffector(const std::string& eef_name);
479 
482  const std::pair<std::string, std::string>& getEndEffectorParentGroup() const
483  {
484  return end_effector_parent_;
485  }
486 
488  const std::vector<std::string>& getAttachedEndEffectorNames() const
489  {
491  }
492 
500  bool getEndEffectorTips(std::vector<const LinkModel*>& tips) const;
501 
509  bool getEndEffectorTips(std::vector<std::string>& tips) const;
510 
517 
519  const JointBoundsVector& getActiveJointModelsBounds() const
520  {
522  }
523 
524  const std::pair<KinematicsSolver, KinematicsSolverMap>& getGroupKinematics() const
525  {
526  return group_kinematics_;
527  }
528 
529  void setSolverAllocators(const SolverAllocatorFn& solver,
530  const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn())
531  {
532  setSolverAllocators(std::make_pair(solver, solver_map));
533  }
534 
535  void setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers);
536 
537  const kinematics::KinematicsBaseConstPtr getSolverInstance() const
538  {
539  return group_kinematics_.first.solver_instance_;
540  }
541 
542  const kinematics::KinematicsBasePtr& getSolverInstance()
543  {
544  return group_kinematics_.first.solver_instance_;
545  }
546 
547  bool canSetStateFromIK(const std::string& tip) const;
548 
549  bool setRedundantJoints(const std::vector<std::string>& joints)
550  {
551  if (group_kinematics_.first.solver_instance_)
552  return group_kinematics_.first.solver_instance_->setRedundantJoints(joints);
553  return false;
554  }
555 
557  double getDefaultIKTimeout() const
558  {
559  return group_kinematics_.first.default_ik_timeout_;
560  }
561 
563  void setDefaultIKTimeout(double ik_timeout);
564 
569  const std::vector<unsigned int>& getKinematicsSolverJointBijection() const
570  {
571  return group_kinematics_.first.bijection_;
572  }
573 
575  void printGroupInfo(std::ostream& out = std::cout) const;
576 
578  bool isValidVelocityMove(const std::vector<double>& from_joint_pose, const std::vector<double>& to_joint_pose,
579  double dt) const;
580 
582  bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size,
583  double dt) const;
584 
585 protected:
586  bool computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
587  std::vector<unsigned int>& joint_bijection) const;
588 
592  void updateMimicJoints(double* values) const;
593 
596 
598  std::string name_;
599 
601  std::vector<const JointModel*> joint_model_vector_;
602 
604  std::vector<std::string> joint_model_name_vector_;
605 
607  std::vector<const JointModel*> active_joint_model_vector_;
608 
610  std::vector<std::string> active_joint_model_name_vector_;
611 
613  std::vector<const JointModel*> fixed_joints_;
614 
616  std::vector<const JointModel*> mimic_joints_;
617 
619  std::vector<const JointModel*> continuous_joint_model_vector_;
620 
623  std::vector<std::string> variable_names_;
624 
627  std::set<std::string> variable_names_set_;
628 
631 
633  std::vector<const JointModel*> joint_roots_;
634 
637 
642 
644  JointBoundsVector active_joint_models_bounds_;
645 
648  std::vector<int> variable_index_list_;
649 
653 
657  std::vector<const LinkModel*> link_model_vector_;
658 
661 
663  std::vector<std::string> link_model_name_vector_;
664 
665  std::vector<const LinkModel*> link_model_with_geometry_vector_;
666 
668  std::vector<std::string> link_model_with_geometry_name_vector_;
669 
672  std::vector<const LinkModel*> updated_link_model_vector_;
673 
676  std::set<const LinkModel*> updated_link_model_set_;
677 
680  std::vector<std::string> updated_link_model_name_vector_;
681 
684  std::set<std::string> updated_link_model_name_set_;
685 
688  std::vector<const LinkModel*> updated_link_model_with_geometry_vector_;
689 
692  std::set<const LinkModel*> updated_link_model_with_geometry_set_;
693 
697 
701 
703  unsigned int variable_count_;
704 
707 
711 
713  std::vector<std::string> subgroup_names_;
714 
716  std::set<std::string> subgroup_names_set_;
717 
719  std::vector<std::string> attached_end_effector_names_;
720 
724  std::pair<std::string, std::string> end_effector_parent_;
725 
727  std::string end_effector_name_;
728 
729  bool is_chain_;
730 
732 
734  {
735  GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o)
736  {
737  }
738  int src;
739  int dest;
740  double factor;
741  double offset;
742  };
743 
744  std::vector<GroupMimicUpdate> group_mimic_update_;
745 
746  std::pair<KinematicsSolver, KinematicsSolverMap> group_kinematics_;
747 
749 
751  std::map<std::string, std::map<std::string, double> > default_states_;
752 
754  std::vector<std::string> default_states_names_;
755 };
756 } // namespace core
757 } // namespace moveit
758 
759 #endif
bool getEndEffectorTips(std::vector< const LinkModel *> &tips) const
Get the unique set of end effector tips included in a particular joint model group as defined by the ...
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const std::vector< double > &distances) const
Compute random values for the state of the joint group.
std::map< std::string, const LinkModel * > LinkModelMapConst
Map of names to const instances for LinkModel.
Definition: link_model.h:64
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...
const std::set< const LinkModel * > & getUpdatedLinkModelsSet() const
Return the same data as getUpdatedLinkModels() but as a set.
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 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...
const std::set< const LinkModel * > & getUpdatedLinkModelsWithGeometrySet() const
Return the same data as getUpdatedLinkModelsWithGeometry() but as a set.
const std::vector< unsigned int > & getKinematicsSolverJointBijection() const
Return the mapping between the order of the joints in this group and the order of the joints in the k...
std::vector< double > values
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
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...
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...
void getSubgroups(std::vector< const JointModelGroup *> &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
const std::set< std::string > & getUpdatedLinkModelsWithGeometryNamesSet() const
Get the names of the links returned by getUpdatedLinkModels()
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
bool isSingleDOFJoints() const
Return true if the group consists only of joints that are single DOF.
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...
const std::vector< const LinkModel * > & getUpdatedLinkModelsWithGeometry() const
Get the names of the links that are to be updated when the state of this group changes. This includes links that are in the kinematic model but outside this group, if those links are descendants of joints in this group that have their values updated.
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
GroupMimicUpdate(int s, int d, double f, double o)
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for the joint group.
const std::vector< std::string > & getLinkModelNamesWithCollisionGeometry() const
Get the names of the links that are part of this joint group and also have geometry associated with t...
std::vector< const JointModel * > joint_roots_
The list of active joint models that are roots in this group.
geometry_msgs::TransformStamped t
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
std::string name_
Name of group.
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
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...
unsigned int active_variable_count_
The number of variables necessary to describe the active joints in this group of joints.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
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 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.
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 JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
std::vector< std::string > link_model_with_geometry_name_vector_
The names of the links in this group that also have geometry.
const srdf::Model::Group & getConfig() const
get the SRDF configuration this group is based on
bool isChain() const
Check if this group is a linear chain.
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_
std::map< std::string, const JointModelGroup * > JointModelGroupMapConst
Map of names to const instances for JointModelGroup.
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.
bool enforcePositionBounds(double *state) const
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
const kinematics::KinematicsBasePtr & getSolverInstance()
std::vector< const JointModel * > mimic_joints_
Joints that mimic other joints.
bool setRedundantJoints(const std::vector< std::string > &joints)
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values, const std::vector< double > &near, double distance) const
Compute random values for the state of the joint 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...
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
boost::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> SolverAllocatorFn
Function type that allocates a kinematics solver for a particular group.
const RobotModel * parent_model_
Owner model.
std::vector< const JointModel::Bounds * > JointBoundsVector
const std::vector< const JointModel * > & getFixedJointModels() const
Get the fixed joints that are part of this group.
const std::string & getName() const
Get the name of the joint group.
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_
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< unsigned int > &joint_bijection) const
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.
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...
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)
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
std::map< const JointModelGroup *, KinematicsSolver > KinematicsSolverMap
Map from group instances to allocator functions & bijections.
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
std::map< std::string, int > VariableIndexMap
Data type for holding mappings from variable names to their position in a state vector.
Definition: joint_model.h:82
std::vector< const JointModel * > fixed_joints_
The joints that have no DOF (fixed)
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
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 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...
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values, const std::vector< double > &near, const std::vector< double > &distances) const
Compute random values for the state of the joint 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...
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints). ...
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
LinkModelMapConst link_model_map_
A map from link names to their instances.
const std::vector< const JointModel * > & getJointRoots() const
Unlike a complete kinematic model, a group may contain disconnected parts of the kinematic tree – a ...
bool isLinkUpdated(const std::string &name) const
True if this name is in the set of links that are to be updated when the state of this group changes...
bool isValidVelocityMove(const std::vector< double > &from_joint_pose, const std::vector< double > &to_joint_pose, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits.
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::map< const JointModelGroup *, SolverAllocatorFn > SolverAllocatorMapFn
Map from group instances to allocator functions & bijections.
const std::vector< std::string > & getUpdatedLinkModelsWithGeometryNames() const
Get the names of the links returned by getUpdatedLinkModels()
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 std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set) ...
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group...
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values) const
Compute random values for the state of the joint group.
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
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.
const std::vector< const LinkModel * > & getUpdatedLinkModels() const
Get the names of the links that are to be updated when the state of this group changes. This includes links that are in the kinematic model but outside this group, if those links are descendants of joints in this group that have their values updated. The order is the correct order for updating the corresponding states.
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group) ...
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values, const std::vector< double > &near, const std::map< JointModel::JointType, double > &distance_map) const
Compute random values for the state of the joint group.
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel *> &joint_vector, const RobotModel *parent_model)
double distance(const double *state1, const double *state2) const
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
std::map< std::string, const JointModel * > JointModelMapConst
Map of names to const instances for JointModel.
Definition: joint_model.h:94
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
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!
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
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...
void interpolate(const double *from, const double *to, double t, double *state) const
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 ...
bool isEndEffector() const
Check if this group was designated as an end-effector in the SRDF.
double getDefaultIKTimeout() const
Get the default IK timeout.
bool canSetStateFromIK(const std::string &tip) const
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
const std::string & getEndEffectorName() const
Return the name of the end effector, if this group is an end-effector.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 17 2022 02:51:04