moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 #pragma once
39 
43 #include <srdfdom/model.h>
44 #include <functional>
45 #include <set>
46 #include <string>
47 
48 namespace moveit
49 {
50 namespace core
51 {
52 class RobotModel;
53 class JointModelGroup;
54 
56 typedef std::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
57 
59 using SolverAllocatorMapFn = std::map<const JointModelGroup*, SolverAllocatorFn>;
60 
62 using JointModelGroupMap = std::map<std::string, JointModelGroup*>;
63 
65 using JointModelGroupMapConst = std::map<std::string, const JointModelGroup*>;
66 
67 using JointBoundsVector = std::vector<const JointModel::Bounds*>;
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 
92 
98  std::vector<size_t> bijection_;
99 
100  kinematics::KinematicsBasePtr solver_instance_;
101 
103  };
104 
106  using KinematicsSolverMap = std::map<const JointModelGroup*, KinematicsSolver>;
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 
126  const srdf::Model::Group& getConfig() const
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 
319  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const
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  }
338  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
339  const std::vector<double>& near, double distance) const
340  {
341  values.resize(variable_count_);
343  }
344 
346  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
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  }
361  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
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 
368  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
369  const JointBoundsVector& active_joint_bounds) const;
370 
372  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
373  const JointBoundsVector& active_joint_bounds, const double* near,
374  const double distance) const;
375 
377  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
378  const JointBoundsVector& active_joint_bounds, const double* near,
379  const std::map<JointModel::JointType, double>& distance_map) const;
380 
382  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
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 
520  {
522  }
523 
524  const std::pair<KinematicsSolver, KinematicsSolverMap>& getGroupKinematics() const
525  {
526  return group_kinematics_;
527  }
528 
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<size_t>& 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, std::vector<size_t>& joint_bijection) const;
587 
591  void updateMimicJoints(double* values) const;
592 
595 
597  std::string name_;
598 
600  std::vector<const JointModel*> joint_model_vector_;
601 
603  std::vector<std::string> joint_model_name_vector_;
604 
606  std::vector<const JointModel*> active_joint_model_vector_;
607 
609  std::vector<std::string> active_joint_model_name_vector_;
610 
612  std::vector<const JointModel*> fixed_joints_;
613 
615  std::vector<const JointModel*> mimic_joints_;
616 
618  std::vector<const JointModel*> continuous_joint_model_vector_;
619 
622  std::vector<std::string> variable_names_;
623 
626  std::set<std::string> variable_names_set_;
627 
630 
632  std::vector<const JointModel*> joint_roots_;
633 
636 
641 
644 
647  std::vector<int> variable_index_list_;
648 
652 
656  std::vector<const LinkModel*> link_model_vector_;
657 
660 
662  std::vector<std::string> link_model_name_vector_;
663 
664  std::vector<const LinkModel*> link_model_with_geometry_vector_;
665 
667  std::vector<std::string> link_model_with_geometry_name_vector_;
668 
671  std::vector<const LinkModel*> updated_link_model_vector_;
672 
675  std::set<const LinkModel*> updated_link_model_set_;
676 
679  std::vector<std::string> updated_link_model_name_vector_;
680 
683  std::set<std::string> updated_link_model_name_set_;
684 
687  std::vector<const LinkModel*> updated_link_model_with_geometry_vector_;
688 
691  std::set<const LinkModel*> updated_link_model_with_geometry_set_;
692 
696 
700 
702  unsigned int variable_count_;
703 
706 
710 
712  std::vector<std::string> subgroup_names_;
713 
715  std::set<std::string> subgroup_names_set_;
716 
718  std::vector<std::string> attached_end_effector_names_;
719 
723  std::pair<std::string, std::string> end_effector_parent_;
724 
726  std::string end_effector_name_;
727 
728  bool is_chain_;
729 
731 
733  {
734  GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o)
735  {
736  }
737  int src;
738  int dest;
739  double factor;
740  double offset;
741  };
742 
743  std::vector<GroupMimicUpdate> group_mimic_update_;
744 
745  std::pair<KinematicsSolver, KinematicsSolverMap> group_kinematics_;
746 
747  srdf::Model::Group config_;
748 
750  std::map<std::string, std::map<std::string, double> > default_states_;
751 
753  std::vector<std::string> default_states_names_;
754 };
755 } // namespace core
756 } // namespace moveit
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...
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::string name_
Name of group.
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< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
const std::vector< const JointModel * > & getFixedJointModels() const
Get the fixed joints that are part of this group.
double getDefaultIKTimeout() const
Get the default IK timeout.
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
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...
bool isChain() const
Check if this group is a linear chain.
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this group contains.
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 ...
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.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::string & getEndEffectorName() const
Return the name of the end effector, if this group is an end-effector.
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
std::vector< const JointModel * > joint_roots_
The list of active joint models that are roots in this group.
void setDefaultIKTimeout(double ik_timeout)
Set the default IK timeout.
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 ...
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 std::string & getName() const
Get the name of the joint group.
std::map< std::string, std::map< std::string, double > > default_states_
The set of default states specified for this group in the SRDF.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
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...
double distance(const double *state1, const double *state2) const
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.
void attachEndEffector(const std::string &eef_name)
Notify this group that there is an end-effector attached to it.
const srdf::Model::Group & getConfig() const
get the SRDF configuration this group is based on
const std::vector< const JointModel * > & getJointRoots() const
Unlike a complete kinematic model, a group may contain disconnected parts of the kinematic tree – a s...
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
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...
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...
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints).
std::vector< std::string > default_states_names_
The names of the default states specified for this group in the SRDF.
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group)
const std::set< const LinkModel * > & getUpdatedLinkModelsSet() const
Return the same data as getUpdatedLinkModels() but as a set.
const std::set< const LinkModel * > & getUpdatedLinkModelsWithGeometrySet() const
Return the same data as getUpdatedLinkModelsWithGeometry() but as a set.
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....
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
const std::set< std::string > & getUpdatedLinkModelsWithGeometryNamesSet() const
Get the names of the links returned by getUpdatedLinkModels()
LinkModelMapConst link_model_map_
A map from link names to their instances.
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...
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel * > &joint_vector, const RobotModel *parent_model)
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...
const std::vector< std::string > & getUpdatedLinkModelsWithGeometryNames() const
Get the names of the links returned by getUpdatedLinkModels()
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...
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group.
bool setRedundantJoints(const std::vector< std::string > &joints)
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.
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...
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...
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::KinematicsBaseConstPtr getSolverInstance() const
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
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 > 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...
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Throw an exception if the link is not part of this group.
void interpolate(const double *from, const double *to, double t, double *state) const
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 > subgroup_names_
The set of labelled subgroups that are included in 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...
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.
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
unsigned int active_variable_count_
The number of variables necessary to describe the active joints in this group of joints.
bool isEndEffector() const
Check if this group was designated as an end-effector in the SRDF.
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints....
std::vector< GroupMimicUpdate > group_mimic_update_
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
unsigned int variable_count_
The number of variables necessary to describe this group of joints.
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for the joint group.
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
std::pair< KinematicsSolver, KinematicsSolverMap > group_kinematics_
bool isSingleDOFJoints() const
Return true if the group consists only of joints that are single DOF.
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::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
bool canSetStateFromIK(const std::string &tip) const
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< size_t > &joint_bijection) const
const std::vector< size_t > & 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< const LinkModel * > link_model_with_geometry_vector_
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...
std::vector< const JointModel * > mimic_joints_
Joints that mimic other joints.
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 > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
std::vector< std::string > link_model_name_vector_
The names of the links in this group.
std::map< const JointModelGroup *, KinematicsSolver > KinematicsSolverMap
Map from group instances to allocator functions & bijections.
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
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::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of 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.
VariableIndexMap joint_variables_index_map_
The group includes all the joint variables that make up the joints the group consists of....
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::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< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set)
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
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...
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
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< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
const RobotModel * parent_model_
Owner model.
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
std::vector< std::string > joint_model_name_vector_
Names of joints in the order they appear in the group state.
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values) const
Compute random values for the state of the joint group.
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....
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...
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....
std::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
const kinematics::KinematicsBasePtr & getSolverInstance()
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 JointModel * > fixed_joints_
The joints that have no DOF (fixed)
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
std::map< const JointModelGroup *, SolverAllocatorFn > SolverAllocatorMapFn
Map from group instances to allocator functions & bijections.
std::map< std::string, size_t > VariableIndexMap
Data type for holding mappings from variable names to their position in a state vector.
Definition: joint_model.h:90
std::map< std::string, const JointModelGroup * > JointModelGroupMapConst
Map of names to const instances for JointModelGroup.
std::map< std::string, const LinkModel * > LinkModelMapConst
Map of names to const instances for LinkModel.
Definition: link_model.h:64
std::map< std::string, const JointModel * > JointModelMapConst
Map of names to const instances for JointModel.
Definition: joint_model.h:102
std::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> SolverAllocatorFn
Function type that allocates a kinematics solver for a particular group.
std::vector< const JointModel::Bounds * > JointBoundsVector
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
Main namespace for MoveIt.
Definition: exceptions.h:43
d
Definition: setup.py:4
name
Definition: setup.py:7
GroupMimicUpdate(int s, int d, double f, double o)
std::vector< size_t > bijection_
The mapping between the order of the joints in the group and the order of the joints in the kinematic...
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.