moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
joint_model_group.hpp
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
48namespace moveit
49{
50namespace core
51{
52class RobotModel;
53class JointModelGroup;
54
56typedef std::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
57
59using SolverAllocatorMapFn = std::map<const JointModelGroup*, SolverAllocatorFn>;
60
62using JointModelGroupMap = std::map<std::string, JointModelGroup*>;
63
65using JointModelGroupMapConst = std::map<std::string, const JointModelGroup*>;
66
67using JointBoundsVector = std::vector<const JointModel::Bounds*>;
68
70{
71public:
73 {
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
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
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 {
289 }
290
292 int getVariableGroupIndex(const std::string& variable) const;
293
295 const std::vector<std::string>& getDefaultStateNames() const
296 {
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 {
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 {
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 {
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
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
586 bool computeJointVariableIndices(const std::vector<std::string>& joint_names,
587 std::vector<size_t>& joint_bijection) const;
588
596 [[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> getLowerAndUpperLimits() const;
597
604 [[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> getMaxVelocitiesAndAccelerationBounds() const;
605
606protected:
610 void updateMimicJoints(double* values) const;
611
614
616 std::string name_;
617
619 std::vector<const JointModel*> joint_model_vector_;
620
622 std::vector<std::string> joint_model_name_vector_;
623
625 std::vector<const JointModel*> active_joint_model_vector_;
626
628 std::vector<std::string> active_joint_model_name_vector_;
629
631 std::vector<const JointModel*> fixed_joints_;
632
634 std::vector<const JointModel*> mimic_joints_;
635
637 std::vector<const JointModel*> continuous_joint_model_vector_;
638
641 std::vector<std::string> variable_names_;
642
645 std::set<std::string> variable_names_set_;
646
649
651 std::vector<const JointModel*> joint_roots_;
652
655
660
663
666 std::vector<int> variable_index_list_;
667
671
675 std::vector<const LinkModel*> link_model_vector_;
676
679
681 std::vector<std::string> link_model_name_vector_;
682
683 std::vector<const LinkModel*> link_model_with_geometry_vector_;
684
687
690 std::vector<const LinkModel*> updated_link_model_vector_;
691
694 std::set<const LinkModel*> updated_link_model_set_;
695
698 std::vector<std::string> updated_link_model_name_vector_;
699
702 std::set<std::string> updated_link_model_name_set_;
703
706 std::vector<const LinkModel*> updated_link_model_with_geometry_vector_;
707
710 std::set<const LinkModel*> updated_link_model_with_geometry_set_;
711
715
719
721 unsigned int variable_count_;
722
725
729
731 std::vector<std::string> subgroup_names_;
732
734 std::set<std::string> subgroup_names_set_;
735
737 std::vector<std::string> attached_end_effector_names_;
738
742 std::pair<std::string, std::string> end_effector_parent_;
743
746
748
750
752 {
753 GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o)
754 {
755 }
756 int src;
757 int dest;
758 double factor;
759 double offset;
760 };
761
762 std::vector<GroupMimicUpdate> group_mimic_update_;
763
764 std::pair<KinematicsSolver, KinematicsSolverMap> group_kinematics_;
765
766 srdf::Model::Group config_;
767
769 std::map<std::string, std::map<std::string, double> > default_states_;
770
772 std::vector<std::string> default_states_names_;
773};
774} // namespace core
775} // 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.
const RobotModel & getParentModel() const
Get the kinematic model this group is part of.
std::vector< const JointModel * > joint_model_vector_
Joint instances in the order they appear in the group state.
double getDefaultIKTimeout() const
Get the default IK timeout.
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
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< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
const srdf::Model::Group & getConfig() const
get the SRDF configuration this group is based on
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
std::vector< const JointModel * > active_joint_model_vector_
Active joint instances in the order they appear in the group state.
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 ...
const std::vector< std::string > & getUpdatedLinkModelsWithGeometryNames() const
Get the names of the links returned by getUpdatedLinkModels()
const std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set)
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::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::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.
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...
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.
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...
bool computeJointVariableIndices(const std::vector< std::string > &joint_names, std::vector< size_t > &joint_bijection) const
Computes the indices of joint variables given a vector of joint names to look up.
const std::string & getName() const
Get the name of the joint group.
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
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::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
void setEndEffectorName(const std::string &name)
Set the name of the end-effector, and remember this group is indeed an end-effector.
LinkModelMapConst link_model_map_
A map from link names to their instances.
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...
const std::set< const LinkModel * > & getUpdatedLinkModelsSet() const
Return the same data as getUpdatedLinkModels() but as a set.
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.
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
const std::vector< const JointModel * > & getJointRoots() const
Unlike a complete kinematic model, a group may contain disconnected parts of the kinematic tree – a s...
JointModelMapConst joint_model_map_
A map from joint names to their instances. This includes all joints in the group.
std::pair< Eigen::VectorXd, Eigen::VectorXd > getLowerAndUpperLimits() const
Get the lower and upper position limits of all active variables in the group.
std::pair< Eigen::VectorXd, Eigen::VectorXd > getMaxVelocitiesAndAccelerationBounds() const
Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group con...
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< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in 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 JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
const std::vector< std::string > & getAttachedEndEffectorNames() const
Get the names of the end effectors attached to this group.
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.
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints).
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 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...
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.
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for the joint group.
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.
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...
void getSubgroups(std::vector< const JointModelGroup * > &sub_groups) const
Get the groups that are subsets of this one (in terms of joints set)
bool canSetStateFromIK(const std::string &tip) const
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...
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.
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< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint 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...
int getVariableGroupIndex(const std::string &variable) const
Get the index of a variable within the group. Return -1 on error.
const std::vector< const JointModel * > & getFixedJointModels() const
Get the fixed joints that are 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....
const std::set< std::string > & getUpdatedLinkModelsWithGeometryNamesSet() const
Get the names of the links returned by getUpdatedLinkModels()
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...
std::vector< std::string > active_joint_model_name_vector_
Names of active joints in the order they appear in the group state.
const std::vector< std::string > & getDefaultStateNames() const
Get the names of the known default states (as specified in the SRDF)
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....
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....
const std::string & getEndEffectorName() const
Return the name of the end effector, if this group is an end-effector.
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::set< std::string > subgroup_names_set_
The set of labelled subgroups that are included in this group.
const std::set< const LinkModel * > & getUpdatedLinkModelsWithGeometrySet() const
Return the same data as getUpdatedLinkModelsWithGeometry() but as a set.
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)
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
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....
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
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.
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.
std::map< std::string, const JointModel * > JointModelMapConst
Map of names to const instances for JointModel.
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.
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.