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 <boost/function.hpp>
45 #include <set>
46 
47 namespace moveit
48 {
49 namespace core
50 {
51 class RobotModel;
52 class JointModelGroup;
53 
55 typedef boost::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
56 
58 using SolverAllocatorMapFn = std::map<const JointModelGroup*, SolverAllocatorFn>;
59 
61 using JointModelGroupMap = std::map<std::string, JointModelGroup*>;
62 
64 using JointModelGroupMapConst = std::map<std::string, const JointModelGroup*>;
65 
66 using JointBoundsVector = std::vector<const JointModel::Bounds*>;
67 
68 class JointModelGroup
69 {
70 public:
71  struct KinematicsSolver
72  {
74  {
75  }
76 
78  operator bool() const
79  {
80  return allocator_ && !bijection_.empty() && solver_instance_;
81  }
82 
83  void reset()
84  {
85  solver_instance_.reset();
86  bijection_.clear();
87  }
88 
91 
97  std::vector<unsigned int> bijection_;
98 
99  kinematics::KinematicsBasePtr solver_instance_;
100 
101  double default_ik_timeout_;
102  };
103 
105  using KinematicsSolverMap = std::map<const JointModelGroup*, KinematicsSolver>;
106 
107  JointModelGroup(const std::string& name, const srdf::Model::Group& config,
108  const std::vector<const JointModel*>& joint_vector, const RobotModel* parent_model);
109 
111 
113  const RobotModel& getParentModel() const
114  {
115  return *parent_model_;
116  }
117 
119  const std::string& getName() const
120  {
121  return name_;
122  }
123 
125  const srdf::Model::Group& getConfig() const
126  {
127  return config_;
128  }
129 
131  bool hasJointModel(const std::string& joint) const;
132 
134  bool hasLinkModel(const std::string& link) const;
135 
137  const JointModel* getJointModel(const std::string& joint) const;
138 
140  const LinkModel* getLinkModel(const std::string& link) const;
141 
143  const std::vector<const JointModel*>& getJointModels() const
144  {
145  return joint_model_vector_;
146  }
147 
150  const std::vector<std::string>& getJointModelNames() const
151  {
153  }
154 
156  const std::vector<const JointModel*>& getActiveJointModels() const
157  {
159  }
160 
163  const std::vector<std::string>& getActiveJointModelNames() const
164  {
166  }
167 
169  const std::vector<const JointModel*>& getFixedJointModels() const
170  {
172  }
173 
175  const std::vector<const JointModel*>& getMimicJointModels() const
176  {
177  return mimic_joints_;
178  }
179 
181  const std::vector<const JointModel*>& getContinuousJointModels() const
182  {
184  }
185 
188  const std::vector<std::string>& getVariableNames() const
189  {
190  return variable_names_;
191  }
192 
200  const std::vector<const JointModel*>& getJointRoots() const
201  {
202  return joint_roots_;
203  }
204 
206  const JointModel* getCommonRoot() const
207  {
208  return common_root_;
209  }
210 
212  const std::vector<const LinkModel*>& getLinkModels() const
213  {
214  return link_model_vector_;
215  }
216 
218  const std::vector<std::string>& getLinkModelNames() const
219  {
221  }
222 
224  const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
225  {
227  }
228 
233  const std::vector<const LinkModel*>& getUpdatedLinkModels() const
234  {
236  }
237 
239  const std::set<const LinkModel*>& getUpdatedLinkModelsSet() const
240  {
242  }
243 
245  const std::vector<std::string>& getUpdatedLinkModelNames() const
246  {
248  }
249 
253  const std::vector<const LinkModel*>& getUpdatedLinkModelsWithGeometry() const
254  {
256  }
257 
259  const std::set<const LinkModel*>& getUpdatedLinkModelsWithGeometrySet() const
260  {
262  }
263 
265  const std::vector<std::string>& getUpdatedLinkModelsWithGeometryNames() const
266  {
268  }
269 
271  const std::set<std::string>& getUpdatedLinkModelsWithGeometryNamesSet() const
272  {
274  }
275 
279  bool isLinkUpdated(const std::string& name) const
280  {
282  }
283 
285  const std::vector<int>& getVariableIndexList() const
286  {
287  return variable_index_list_;
288  }
289 
291  int getVariableGroupIndex(const std::string& variable) const;
292 
294  const std::vector<std::string>& getDefaultStateNames() const
295  {
296  return default_states_names_;
297  }
298 
299  void addDefaultState(const std::string& name, const std::map<std::string, double>& default_state);
300 
302  bool getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const;
303 
305  void getVariableDefaultPositions(std::map<std::string, double>& values) const;
306 
308  void getVariableDefaultPositions(std::vector<double>& values) const
309  {
310  values.resize(variable_count_);
312  }
313 
315  void getVariableDefaultPositions(double* values) const;
316 
319  {
321  }
322 
324  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
325  {
326  values.resize(variable_count_);
328  }
329 
331  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* seed,
332  const double distance) const
333  {
335  }
338  const std::vector<double>& seed, double distance) const
339  {
340  values.resize(variable_count_);
342  }
343 
346  const std::vector<double>& seed,
347  const std::map<JointModel::JointType, double>& distance_map) const
348  {
349  values.resize(variable_count_);
350  getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &seed[0], distance_map);
351  }
352 
354  void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* seed,
355  const std::vector<double>& distances) const
356  {
358  }
361  const std::vector<double>& seed, const std::vector<double>& distances) const
362  {
363  values.resize(variable_count_);
365  }
366 
368  const JointBoundsVector& active_joint_bounds) const;
369 
372  const JointBoundsVector& active_joint_bounds, const double* seed,
373  const double distance) const;
374 
377  const JointBoundsVector& active_joint_bounds, const double* seed,
378  const std::map<JointModel::JointType, double>& distance_map) const;
379 
382  const JointBoundsVector& active_joint_bounds, const double* seed,
383  const std::vector<double>& distances) const;
384 
385  bool enforcePositionBounds(double* state) const
386  {
388  }
389 
390  bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const;
391  bool satisfiesPositionBounds(const double* state, double margin = 0.0) const
392  {
394  }
395  bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
396  double margin = 0.0) const;
397 
398  double getMaximumExtent() const
399  {
401  }
402  double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
403 
404  double distance(const double* state1, const double* state2) const;
405  void interpolate(const double* from, const double* to, double t, double* state) const;
406 
409  unsigned int getVariableCount() const
410  {
412  }
413 
416  unsigned int getActiveVariableCount() const
417  {
418  return active_variable_count_;
419  }
420 
422  void setSubgroupNames(const std::vector<std::string>& subgroups);
423 
425  const std::vector<std::string>& getSubgroupNames() const
426  {
427  return subgroup_names_;
428  }
429 
431  void getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const;
432 
434  bool isSubgroup(const std::string& group) const
435  {
436  return subgroup_names_set_.find(group) != subgroup_names_set_.end();
437  }
438 
440  bool isChain() const
441  {
442  return is_chain_;
443  }
444 
446  bool isSingleDOFJoints() const
447  {
448  return is_single_dof_;
449  }
450 
452  bool isEndEffector() const
453  {
454  return !end_effector_name_.empty();
455  }
456 
458  {
460  }
461 
463  const std::string& getEndEffectorName() const
464  {
465  return end_effector_name_;
466  }
467 
469  void setEndEffectorName(const std::string& name);
470 
474  void setEndEffectorParent(const std::string& group, const std::string& link);
475 
477  void attachEndEffector(const std::string& eef_name);
478 
481  const std::pair<std::string, std::string>& getEndEffectorParentGroup() const
482  {
483  return end_effector_parent_;
484  }
485 
487  const std::vector<std::string>& getAttachedEndEffectorNames() const
488  {
490  }
491 
499  bool getEndEffectorTips(std::vector<const LinkModel*>& tips) const;
500 
508  bool getEndEffectorTips(std::vector<std::string>& tips) const;
509 
516 
519  {
521  }
522 
523  const std::pair<KinematicsSolver, KinematicsSolverMap>& getGroupKinematics() const
524  {
525  return group_kinematics_;
526  }
527 
528  void setSolverAllocators(const SolverAllocatorFn& solver,
530  {
531  setSolverAllocators(std::make_pair(solver, solver_map));
532  }
533 
534  void setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers);
535 
536  const kinematics::KinematicsBaseConstPtr getSolverInstance() const
537  {
538  return group_kinematics_.first.solver_instance_;
539  }
540 
541  const kinematics::KinematicsBasePtr& getSolverInstance()
542  {
543  return group_kinematics_.first.solver_instance_;
544  }
545 
546  bool canSetStateFromIK(const std::string& tip) const;
547 
548  bool setRedundantJoints(const std::vector<std::string>& joints)
549  {
550  if (group_kinematics_.first.solver_instance_)
551  return group_kinematics_.first.solver_instance_->setRedundantJoints(joints);
552  return false;
553  }
554 
556  double getDefaultIKTimeout() const
557  {
558  return group_kinematics_.first.default_ik_timeout_;
559  }
560 
562  void setDefaultIKTimeout(double ik_timeout);
563 
568  const std::vector<unsigned int>& getKinematicsSolverJointBijection() const
569  {
570  return group_kinematics_.first.bijection_;
571  }
572 
574  void printGroupInfo(std::ostream& out = std::cout) const;
575 
577  bool isValidVelocityMove(const std::vector<double>& from_joint_pose, const std::vector<double>& to_joint_pose,
578  double dt) const;
579 
581  bool isValidVelocityMove(const double* from_joint_pose, const double* to_joint_pose, std::size_t array_size,
582  double dt) const;
583 
584 protected:
585  bool computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
586  std::vector<unsigned int>& 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 
635  const JointModel* common_root_;
636 
641 
644 
647  std::vector<int> variable_index_list_;
648 
651  std::vector<int> active_joint_model_start_index_;
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 
699  std::set<std::string> updated_link_model_with_geometry_name_set_;
700 
702  unsigned int variable_count_;
703 
705  unsigned int active_variable_count_;
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 
732  struct GroupMimicUpdate
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 
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
moveit::core::JointModelGroup::hasLinkModel
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
Definition: joint_model_group.cpp:354
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core::JointModelGroup::KinematicsSolver::bijection_
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...
Definition: joint_model_group.h:163
moveit::core::JointModelGroup::updated_link_model_with_geometry_vector_
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...
Definition: joint_model_group.h:753
moveit::core::JointModelGroup::getActiveJointModels
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
Definition: joint_model_group.h:222
moveit::core::JointModelGroup::getJointModel
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.
Definition: joint_model_group.cpp:370
moveit::core::JointModelGroup::mimic_joints_
std::vector< const JointModel * > mimic_joints_
Joints that mimic other joints.
Definition: joint_model_group.h:681
moveit::core::JointModelGroup::getUpdatedLinkModelsWithGeometry
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....
Definition: joint_model_group.h:319
moveit::core::JointModelGroup::addDefaultState
void addDefaultState(const std::string &name, const std::map< std::string, double > &default_state)
Definition: joint_model_group.cpp:506
moveit::core::JointModelGroup::getActiveVariableCount
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
Definition: joint_model_group.h:482
moveit::core::JointModelGroup::getSubgroupNames
const std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set)
Definition: joint_model_group.h:491
moveit::core::JointModelGroup::getJointRoots
const std::vector< const JointModel * > & getJointRoots() const
Unlike a complete kinematic model, a group may contain disconnected parts of the kinematic tree – a s...
Definition: joint_model_group.h:266
moveit::core::JointModelGroup::getEndEffectorName
const std::string & getEndEffectorName() const
Return the name of the end effector, if this group is an end-effector.
Definition: joint_model_group.h:529
moveit::core::JointModelGroup::getVariableIndexList
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
Definition: joint_model_group.h:351
moveit::core::JointModelGroup::getActiveJointModelNames
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...
Definition: joint_model_group.h:229
moveit::core::JointModelGroup::variable_index_list_
std::vector< int > variable_index_list_
The list of index values this group includes, with respect to a full robot state; this includes mimic...
Definition: joint_model_group.h:713
moveit::core::JointModelGroup::getGroupKinematics
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
Definition: joint_model_group.h:589
moveit::core::JointModelGroup::getLinkModel
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.
Definition: joint_model_group.cpp:359
moveit::core::JointModelGroup::KinematicsSolver::allocator_
SolverAllocatorFn allocator_
Function type that allocates a kinematics solver for a particular group.
Definition: joint_model_group.h:156
s
XmlRpcServer s
moveit::core::JointModelGroup::getUpdatedLinkModelsWithGeometrySet
const std::set< const LinkModel * > & getUpdatedLinkModelsWithGeometrySet() const
Return the same data as getUpdatedLinkModelsWithGeometry() but as a set.
Definition: joint_model_group.h:325
moveit::core::JointModelGroup::GroupMimicUpdate::dest
int dest
Definition: joint_model_group.h:804
moveit::core::VariableIndexMap
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:147
moveit::core::JointModelGroup::updated_link_model_with_geometry_name_set_
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...
Definition: joint_model_group.h:765
moveit::core::JointModelGroup::getActiveJointModelsBounds
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
Definition: joint_model_group.h:584
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:143
moveit::core::JointModelGroup::active_joint_model_start_index_
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...
Definition: joint_model_group.h:717
moveit::core::JointModelGroup::continuous_joint_model_vector_
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this group contains.
Definition: joint_model_group.h:684
moveit::core::JointModelGroup::active_joint_model_vector_
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
Definition: joint_model_group.h:672
moveit::core::JointModelGroup::default_states_names_
std::vector< std::string > default_states_names_
The names of the default states specified for this group in the SRDF.
Definition: joint_model_group.h:819
moveit::core::JointModelGroup::updateMimicJoints
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints....
Definition: joint_model_group.cpp:499
moveit::core::JointModelGroup::updated_link_model_with_geometry_name_vector_
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...
Definition: joint_model_group.h:761
moveit::core::JointModelGroup::isValidVelocityMove
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.
Definition: joint_model_group.cpp:788
moveit::core::JointModelGroup::updated_link_model_vector_
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...
Definition: joint_model_group.h:737
moveit::core::JointModelGroup::link_model_vector_
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 ...
Definition: joint_model_group.h:722
moveit::core::JointModelGroup::parent_model_
const RobotModel * parent_model_
Owner model.
Definition: joint_model_group.h:660
kinematics_base.h
moveit::core::JointModelGroup::getParentModel
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
Definition: joint_model_group.h:179
moveit::core::JointBoundsVector
std::vector< const JointModel::Bounds * > JointBoundsVector
Definition: joint_model_group.h:132
moveit::core::JointModelGroup::setRedundantJoints
bool setRedundantJoints(const std::vector< std::string > &joints)
Definition: joint_model_group.h:614
moveit::core::JointModelGroup::config_
srdf::Model::Group config_
Definition: joint_model_group.h:813
moveit::core::JointModelGroup::getMimicJointModels
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
Definition: joint_model_group.h:241
moveit::core::JointModelGroup::subgroup_names_set_
std::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
Definition: joint_model_group.h:781
moveit::core::JointModelGroup::getUpdatedLinkModelsWithGeometryNamesSet
const std::set< std::string > & getUpdatedLinkModelsWithGeometryNamesSet() const
Get the names of the links returned by getUpdatedLinkModels()
Definition: joint_model_group.h:337
moveit::core::JointModelGroup::canSetStateFromIK
bool canSetStateFromIK(const std::string &tip) const
Definition: joint_model_group.cpp:683
moveit::core::JointModelGroupMapConst
std::map< std::string, const JointModelGroup * > JointModelGroupMapConst
Map of names to const instances for JointModelGroup.
Definition: joint_model_group.h:130
moveit::core::JointModelGroup::printGroupInfo
void printGroupInfo(std::ostream &out=std::cout) const
Print information about the constructed model.
Definition: joint_model_group.cpp:730
moveit::core::JointModelGroup::joint_model_name_vector_
std::vector< std::string > joint_model_name_vector_
Names of joints in the order they appear in the group state.
Definition: joint_model_group.h:669
moveit::core::JointModelGroup::setEndEffectorParent
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...
Definition: joint_model_group.cpp:541
moveit::core::JointModelGroup::getLinkModelNamesWithCollisionGeometry
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...
Definition: joint_model_group.h:290
f
f
moveit::core::JointModelGroup::getLinkModels
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
Definition: joint_model_group.h:278
moveit::core::SolverAllocatorMapFn
std::map< const JointModelGroup *, SolverAllocatorFn > SolverAllocatorMapFn
Map from group instances to allocator functions & bijections.
Definition: joint_model_group.h:124
moveit::core::JointModelGroup::getAttachedEndEffectorNames
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
Definition: joint_model_group.h:553
moveit::core::JointModelGroup::getEndEffectorTips
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 ...
Definition: joint_model_group.cpp:566
moveit::core::JointModelGroup::getVariableNames
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...
Definition: joint_model_group.h:254
moveit::core::JointModelGroup::attachEndEffector
void attachEndEffector(const std::string &eef_name)
Notify this group that there is an end-effector attached to it.
Definition: joint_model_group.cpp:547
moveit::core::JointModelGroup::KinematicsSolver::solver_instance_
kinematics::KinematicsBasePtr solver_instance_
Definition: joint_model_group.h:165
moveit::core::JointModelGroup::getName
const std::string & getName() const
Get the name of the joint group.
Definition: joint_model_group.h:185
moveit::core::JointModelGroup::variable_count_
unsigned int variable_count_
The number of variables necessary to describe this group of joints.
Definition: joint_model_group.h:768
moveit::core::JointModelGroup::getVariableDefaultPositions
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.
Definition: joint_model_group.cpp:512
moveit::core::JointModelGroup::link_model_map_
LinkModelMapConst link_model_map_
A map from link names to their instances.
Definition: joint_model_group.h:725
moveit::core::JointModelGroup::updated_link_model_name_vector_
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...
Definition: joint_model_group.h:745
moveit::core::JointModelGroup::is_chain_
bool is_chain_
Definition: joint_model_group.h:794
moveit::core::JointModelGroup::getContinuousJointModels
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints).
Definition: joint_model_group.h:247
moveit::core::JointModelGroup::end_effector_parent_
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...
Definition: joint_model_group.h:789
moveit::core::JointModelGroup::KinematicsSolverMap
std::map< const JointModelGroup *, KinematicsSolver > KinematicsSolverMap
Map from group instances to allocator functions & bijections.
Definition: joint_model_group.h:171
moveit::core::JointModelGroup::name_
std::string name_
Name of group.
Definition: joint_model_group.h:663
moveit::core::JointModelGroup::common_root_
const JointModel * common_root_
The joint that is a common root for all joints in this group (not necessarily part of this group)
Definition: joint_model_group.h:701
moveit::core::JointModelGroup::updated_link_model_with_geometry_set_
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...
Definition: joint_model_group.h:757
moveit::core::SolverAllocatorFn
boost::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> SolverAllocatorFn
Function type that allocates a kinematics solver for a particular group.
Definition: joint_model_group.h:118
moveit::core::JointModelGroup::active_variable_count_
unsigned int active_variable_count_
The number of variables necessary to describe the active joints in this group of joints.
Definition: joint_model_group.h:771
moveit::core::JointModelGroup::interpolate
void interpolate(const double *from, const double *to, double t, double *state) const
Definition: joint_model_group.cpp:487
moveit::core::JointModelGroup::default_states_
std::map< std::string, std::map< std::string, double > > default_states_
The set of default states specified for this group in the SRDF.
Definition: joint_model_group.h:816
moveit::core::JointModelGroup::getJointModelNames
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...
Definition: joint_model_group.h:216
moveit::core::JointModelGroup::group_mimic_update_
std::vector< GroupMimicUpdate > group_mimic_update_
Definition: joint_model_group.h:809
moveit::core::JointModelGroup::end_effector_name_
std::string end_effector_name_
The name of the end effector, if this group is an end-effector.
Definition: joint_model_group.h:792
moveit::core::JointModelGroup::link_model_with_geometry_name_vector_
std::vector< std::string > link_model_with_geometry_name_vector_
The names of the links in this group that also have geometry.
Definition: joint_model_group.h:733
moveit::core::JointModelGroup::getCommonRoot
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
Definition: joint_model_group.h:272
moveit::core::JointModelGroup::distance
double distance(const double *state1, const double *state2) const
Definition: joint_model_group.cpp:477
moveit::core::JointModelGroup::getFixedJointModels
const std::vector< const JointModel * > & getFixedJointModels() const
Get the fixed joints that are part of this group.
Definition: joint_model_group.h:235
model.h
moveit::core::JointModelGroup::getUpdatedLinkModelNames
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
Definition: joint_model_group.h:311
setup.d
d
Definition: setup.py:8
moveit::core::JointModelGroup::GroupMimicUpdate::factor
double factor
Definition: joint_model_group.h:805
moveit::core::JointModelGroup::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute random values for the state of the joint group.
Definition: joint_model_group.h:384
moveit::core::JointModelGroup::group_kinematics_
std::pair< KinematicsSolver, KinematicsSolverMap > group_kinematics_
Definition: joint_model_group.h:811
moveit::core::JointModelGroup::getDefaultStateNames
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
Definition: joint_model_group.h:360
moveit::core::LinkModelMapConst
std::map< std::string, const LinkModel * > LinkModelMapConst
Map of names to const instances for LinkModel.
Definition: link_model.h:64
moveit::core::JointModelGroup::getVariableRandomPositionsNearBy
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const double *seed, const double distance) const
Compute random values for the state of the joint group.
Definition: joint_model_group.h:397
moveit::core::JointModelGroup::joint_roots_
std::vector< const JointModel * > joint_roots_
The list of active joint models that are roots in this group.
Definition: joint_model_group.h:698
moveit::core::JointModelGroup::getJointModels
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Definition: joint_model_group.h:209
moveit::core::JointModelGroup::attached_end_effector_names_
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...
Definition: joint_model_group.h:784
moveit::core::JointModelGroup::GroupMimicUpdate::offset
double offset
Definition: joint_model_group.h:806
moveit::core::JointModelGroup::getSubgroups
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
Definition: joint_model_group.cpp:342
moveit::core::JointModelGroup::hasJointModel
bool hasJointModel(const std::string &joint) const
Check if a joint is part of this group.
Definition: joint_model_group.cpp:349
moveit::core::JointModelGroup::is_single_dof_
bool is_single_dof_
Definition: joint_model_group.h:796
moveit::core::JointModelGroup::GroupMimicUpdate::GroupMimicUpdate
GroupMimicUpdate(int s, int d, double f, double o)
Definition: joint_model_group.h:800
random_numbers::RandomNumberGenerator
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
Definition: joint_model_group.h:284
moveit::core::JointModelGroup::satisfiesPositionBounds
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: joint_model_group.h:457
moveit::core::JointModelGroup::JointModelGroup
JointModelGroup(const std::string &name, const srdf::Model::Group &config, const std::vector< const JointModel * > &joint_vector, const RobotModel *parent_model)
Definition: joint_model_group.cpp:167
moveit::core::JointModelGroup::KinematicsSolver::default_ik_timeout_
double default_ik_timeout_
Definition: joint_model_group.h:167
moveit::core::JointModelGroup::joint_variables_index_map_
VariableIndexMap joint_variables_index_map_
The group includes all the joint variables that make up the joints the group consists of....
Definition: joint_model_group.h:706
moveit::core::JointModelGroup::updated_link_model_set_
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...
Definition: joint_model_group.h:741
moveit::core::JointModelGroup::setEndEffectorName
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
Definition: joint_model_group.cpp:536
moveit::core::JointModelGroup::setSolverAllocators
void setSolverAllocators(const SolverAllocatorFn &solver, const SolverAllocatorMapFn &solver_map=SolverAllocatorMapFn())
Definition: joint_model_group.h:594
moveit::core::JointModelGroup::getDefaultIKTimeout
double getDefaultIKTimeout() const
Get the default IK timeout.
Definition: joint_model_group.h:622
moveit::core::JointModelGroup::GroupMimicUpdate::src
int src
Definition: joint_model_group.h:803
moveit::core::JointModelGroup::~JointModelGroup
~JointModelGroup()
moveit::core::JointModelGroup::setSubgroupNames
void setSubgroupNames(const std::vector< std::string > &subgroups)
Set the names of the subgroups for this group.
Definition: joint_model_group.cpp:334
moveit::core::JointModelGroup::variable_names_set_
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...
Definition: joint_model_group.h:692
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
moveit::core::JointModelGroup::getUpdatedLinkModelsSet
const std::set< const LinkModel * > & getUpdatedLinkModelsSet() const
Return the same data as getUpdatedLinkModels() but as a set.
Definition: joint_model_group.h:305
values
std::vector< double > values
moveit::core::JointModelGroup::link_model_name_vector_
std::vector< std::string > link_model_name_vector_
The names of the links in this group.
Definition: joint_model_group.h:728
moveit::core::JointModelGroup::getSolverInstance
const kinematics::KinematicsBaseConstPtr getSolverInstance() const
Definition: joint_model_group.h:602
moveit::core::JointModelGroup::isEndEffector
bool isEndEffector() const
Check if this group was designated as an end-effector in the SRDF.
Definition: joint_model_group.h:518
moveit::core::JointModelGroup::fixed_joints_
std::vector< const JointModel * > fixed_joints_
The joints that have no DOF (fixed)
Definition: joint_model_group.h:678
moveit::core::JointModelGroup::getUpdatedLinkModels
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....
Definition: joint_model_group.h:299
moveit::core::JointModelGroup::active_joint_model_name_vector_
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
Definition: joint_model_group.h:675
moveit::core::JointModelGroup::subgroup_names_
std::vector< std::string > subgroup_names_
The set of labelled subgroups that are included in this group.
Definition: joint_model_group.h:778
moveit::core::JointModelMapConst
std::map< std::string, const JointModel * > JointModelMapConst
Map of names to const instances for JointModel.
Definition: joint_model.h:159
moveit::core::JointModelGroup::isChain
bool isChain() const
Check if this group is a linear chain.
Definition: joint_model_group.h:506
moveit::core::JointModelGroup::GroupMimicUpdate
Definition: joint_model_group.h:798
moveit::core::JointModelGroup::setDefaultIKTimeout
void setDefaultIKTimeout(double ik_timeout)
Set the default IK timeout.
Definition: joint_model_group.cpp:618
moveit::core::JointModelGroup::link_model_with_geometry_vector_
std::vector< const LinkModel * > link_model_with_geometry_vector_
Definition: joint_model_group.h:730
moveit::core::JointModelGroup::joint_model_vector_
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
Definition: joint_model_group.h:666
moveit::core::JointModelGroup::joint_model_map_
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group.
Definition: joint_model_group.h:695
joint_model.h
moveit::core::JointModelGroup::getOnlyOneEndEffectorTip
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...
Definition: joint_model_group.cpp:593
moveit::core::JointModelGroup::getKinematicsSolverJointBijection
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...
Definition: joint_model_group.h:634
moveit::core::JointModelGroup::getUpdatedLinkModelsWithGeometryNames
const std::vector< std::string > & getUpdatedLinkModelsWithGeometryNames() const
Get the names of the links returned by getUpdatedLinkModels()
Definition: joint_model_group.h:331
moveit::core::JointModelGroupMap
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
Definition: joint_model_group.h:127
srdf::Model::Group
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition: joint_model_group.h:475
moveit::core::JointModelGroup::variable_names_
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...
Definition: joint_model_group.h:688
moveit::core::JointModelGroup::is_contiguous_index_list_
bool is_contiguous_index_list_
True if the state of this group is contiguous within the full robot state; this also means that the i...
Definition: joint_model_group.h:775
moveit::core::JointModelGroup::KinematicsSolver::KinematicsSolver
KinematicsSolver()
Definition: joint_model_group.h:139
moveit::core::JointModelGroup::enforcePositionBounds
bool enforcePositionBounds(double *state) const
Definition: joint_model_group.h:451
moveit::core::JointModelGroup::getEndEffectorParentGroup
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...
Definition: joint_model_group.h:547
moveit::core::JointModelGroup::isSingleDOFJoints
bool isSingleDOFJoints() const
Return true if the group consists only of joints that are single DOF.
Definition: joint_model_group.h:512
moveit::core::JointModelGroup::computeIKIndexBijection
bool computeIKIndexBijection(const std::vector< std::string > &ik_jnames, std::vector< unsigned int > &joint_bijection) const
Definition: joint_model_group.cpp:627
moveit::core::JointModelGroup::updated_link_model_name_set_
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...
Definition: joint_model_group.h:749
moveit::core::JointModelGroup::KinematicsSolver::reset
void reset()
Definition: joint_model_group.h:149
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
moveit::core::JointModelGroup::getVariableGroupIndex
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
Definition: joint_model_group.cpp:607
moveit::core::JointModelGroup::isLinkUpdated
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....
Definition: joint_model_group.h:345
moveit::core::JointModelGroup::isContiguousWithinState
bool isContiguousWithinState() const
Definition: joint_model_group.h:523
moveit::core::JointModelGroup::getMaximumExtent
double getMaximumExtent() const
Definition: joint_model_group.h:464
moveit::core::JointModelGroup::getConfig
const srdf::Model::Group & getConfig() const
get the SRDF configuration this group is based on
Definition: joint_model_group.h:191
moveit::core::JointModelGroup::active_joint_models_bounds_
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: joint_model_group.h:709
moveit::core::JointModelGroup::isSubgroup
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
Definition: joint_model_group.h:500


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Mar 3 2024 03:23:35