moveit2
The MoveIt Motion Planning Framework for ROS 2.
robot_model.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 
42 #include <srdfdom/model.h>
43 
44 // joint types
51 #include <rclcpp/logging.hpp>
52 #include <Eigen/Geometry>
53 #include <iostream>
54 
56 namespace moveit
57 {
59 namespace core
60 {
61 MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc
62 
63 static inline void checkInterpolationParamBounds(const rclcpp::Logger& LOGGER, double t)
64 {
65  if (std::isnan(t) || std::isinf(t))
66  {
67  throw Exception("Interpolation parameter is NaN or inf.");
68  }
69 
70  RCLCPP_WARN_STREAM_EXPRESSION(LOGGER, t < 0. || t > 1., "Interpolation parameter is not in the range [0, 1]: " << t);
71 }
72 
76 {
77 public:
79  RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model);
80 
82  ~RobotModel();
83 
85  const std::string& getName() const
86  {
87  return model_name_;
88  }
89 
94  const std::string& getModelFrame() const
95  {
96  return model_frame_;
97  }
98 
100  bool isEmpty() const
101  {
102  return root_link_ == nullptr;
103  }
104 
106  const urdf::ModelInterfaceSharedPtr& getURDF() const
107  {
108  return urdf_;
109  }
110 
112  const srdf::ModelConstSharedPtr& getSRDF() const
113  {
114  return srdf_;
115  }
116 
118  void printModelInfo(std::ostream& out) const;
119 
128  const JointModel* getRootJoint() const;
129 
131  const std::string& getRootJointName() const
132  {
133  return getRootJoint()->getName();
134  }
135 
137  bool hasJointModel(const std::string& name) const;
138 
140  const JointModel* getJointModel(const std::string& joint) const;
141 
143  const JointModel* getJointModel(size_t index) const;
144 
146  JointModel* getJointModel(const std::string& joint);
147 
150  const std::vector<const JointModel*>& getJointModels() const
151  {
153  }
154 
158  const std::vector<JointModel*>& getJointModels()
159  {
160  return joint_model_vector_;
161  }
162 
164  const std::vector<std::string>& getJointModelNames() const
165  {
167  }
168 
170  const std::vector<const JointModel*>& getActiveJointModels() const
171  {
173  }
174 
176  const std::vector<std::string>& getActiveJointModelNames() const
177  {
179  }
180 
182  const std::vector<JointModel*>& getActiveJointModels()
183  {
185  }
186 
188  const std::vector<const JointModel*>& getSingleDOFJointModels() const
189  {
190  return single_dof_joints_;
191  }
192 
194  const std::vector<const JointModel*>& getMultiDOFJointModels() const
195  {
196  return multi_dof_joints_;
197  }
198 
201  const std::vector<const JointModel*>& getContinuousJointModels() const
202  {
204  }
205 
208  const std::vector<const JointModel*>& getMimicJointModels() const
209  {
210  return mimic_joints_;
211  }
212 
213  const JointModel* getJointOfVariable(int variable_index) const
214  {
215  return joints_of_variable_[variable_index];
216  }
217 
218  const JointModel* getJointOfVariable(const std::string& variable) const
219  {
220  return joints_of_variable_[getVariableIndex(variable)];
221  }
222 
223  std::size_t getJointModelCount() const
224  {
225  return joint_model_vector_.size();
226  }
227 
235  const LinkModel* getRootLink() const;
236 
238  const std::string& getRootLinkName() const
239  {
240  return getRootLink()->getName();
241  }
242 
246  bool hasLinkModel(const std::string& name) const;
247 
249  const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const;
250 
252  const LinkModel* getLinkModel(size_t index) const;
253 
255  LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr);
256 
271  static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
272  const JointModelGroup* jmg = nullptr);
273 
275  const std::vector<const LinkModel*>& getLinkModels() const
276  {
278  }
279 
281  const std::vector<LinkModel*>& getLinkModels()
282  {
283  return link_model_vector_;
284  }
285 
287  const std::vector<std::string>& getLinkModelNames() const
288  {
290  }
291 
293  const std::vector<const LinkModel*>& getLinkModelsWithCollisionGeometry() const
294  {
296  }
297 
299  const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
300  {
302  }
303 
304  std::size_t getLinkModelCount() const
305  {
306  return link_model_vector_.size();
307  }
308 
309  std::size_t getLinkGeometryCount() const
310  {
311  return link_geometry_count_;
312  }
313 
317  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const;
318 
320  void getVariableDefaultPositions(double* values) const;
321 
323  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
324  {
325  values.resize(variable_count_);
326  getVariableRandomPositions(rng, &values[0]);
327  }
328 
330  void getVariableDefaultPositions(std::vector<double>& values) const
331  {
332  values.resize(variable_count_);
333  getVariableDefaultPositions(&values[0]);
334  }
335 
337  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
338  std::map<std::string, double>& values) const;
339 
341  void getVariableDefaultPositions(std::map<std::string, double>& values) const;
342 
343  bool enforcePositionBounds(double* state) const
344  {
346  }
347  bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const;
348  bool satisfiesPositionBounds(const double* state, double margin = 0.0) const
349  {
351  }
352  bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
353  double margin = 0.0) const;
354  double getMaximumExtent() const
355  {
357  }
358  double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
359 
361  double distance(const double* state1, const double* state2) const;
362 
371  void interpolate(const double* from, const double* to, double t, double* state) const;
372 
378  bool hasJointModelGroup(const std::string& group) const;
379 
381  const JointModelGroup* getJointModelGroup(const std::string& name) const;
382 
384  JointModelGroup* getJointModelGroup(const std::string& name);
385 
387  const std::vector<const JointModelGroup*>& getJointModelGroups() const
388  {
390  }
391 
393  const std::vector<JointModelGroup*>& getJointModelGroups()
394  {
395  return joint_model_groups_;
396  }
397 
399  const std::vector<std::string>& getJointModelGroupNames() const
400  {
402  }
403 
405  bool hasEndEffector(const std::string& eef) const;
406 
408  const JointModelGroup* getEndEffector(const std::string& name) const;
409 
411  JointModelGroup* getEndEffector(const std::string& name);
412 
414  const std::vector<const JointModelGroup*>& getEndEffectors() const
415  {
416  return end_effectors_;
417  }
418 
422  std::size_t getVariableCount() const
423  {
424  return variable_count_;
425  }
426 
431  const std::vector<std::string>& getVariableNames() const
432  {
433  return variable_names_;
434  }
435 
437  const VariableBounds& getVariableBounds(const std::string& variable) const
438  {
439  return getJointOfVariable(variable)->getVariableBounds(variable);
440  }
441 
444  {
446  }
447 
448  void getMissingVariableNames(const std::vector<std::string>& variables,
449  std::vector<std::string>& missing_variables) const;
450 
452  size_t getVariableIndex(const std::string& variable) const;
453 
455  const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const
456  {
457  if (!a)
458  return b;
459  if (!b)
460  return a;
461  return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]];
462  }
463 
465  void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators);
466 
467 protected:
469  void computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
470  LinkTransformMap& associated_transforms);
471 
473  const JointModel* computeCommonRoot(const JointModel* a, const JointModel* b) const;
474 
476  void updateMimicJoints(double* values) const;
477 
478  // GENERIC INFO
479 
481  std::string model_name_;
482 
485  std::string model_frame_;
486 
487  srdf::ModelConstSharedPtr srdf_;
488 
489  urdf::ModelInterfaceSharedPtr urdf_;
490 
491  // LINKS
492 
495 
498 
500  std::vector<LinkModel*> link_model_vector_;
501 
503  std::vector<const LinkModel*> link_model_vector_const_;
504 
506  std::vector<std::string> link_model_names_vector_;
507 
509  std::vector<const LinkModel*> link_models_with_collision_geometry_vector_;
510 
513 
515  std::size_t link_geometry_count_;
516 
517  // JOINTS
518 
521 
524 
526  std::vector<JointModel*> joint_model_vector_;
527 
529  std::vector<const JointModel*> joint_model_vector_const_;
530 
532  std::vector<std::string> joint_model_names_vector_;
533 
535  std::vector<JointModel*> active_joint_model_vector_;
536 
538  std::vector<std::string> active_joint_model_names_vector_;
539 
541  std::vector<const JointModel*> active_joint_model_vector_const_;
542 
544  std::vector<const JointModel*> continuous_joint_model_vector_;
545 
547  std::vector<const JointModel*> mimic_joints_;
548 
549  std::vector<const JointModel*> single_dof_joints_;
550 
551  std::vector<const JointModel*> multi_dof_joints_;
552 
560  std::vector<int> common_joint_roots_;
561 
562  // INDEXING
563 
566  std::vector<std::string> variable_names_;
567 
569  std::size_t variable_count_;
570 
575 
577 
580 
582  std::vector<const JointModel*> joints_of_variable_;
583 
584  // GROUPS
585 
588 
591 
593  std::vector<JointModelGroup*> joint_model_groups_;
594 
596  std::vector<const JointModelGroup*> joint_model_groups_const_;
597 
599  std::vector<std::string> joint_model_group_names_;
600 
602  std::vector<const JointModelGroup*> end_effectors_;
603 
604 private:
606  void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model);
607 
609  void buildGroups(const srdf::Model& srdf_model);
610 
612  void buildGroupsInfoSubgroups();
613 
615  void buildGroupsInfoEndEffectors(const srdf::Model& srdf_model);
616 
618  void buildMimic(const urdf::ModelInterface& urdf_model);
619 
621  void buildGroupStates(const srdf::Model& srdf_model);
622 
624  void buildJointInfo();
625 
627  void computeDescendants();
628 
630  void computeCommonRoots();
631 
634  JointModel* buildRecursive(LinkModel* parent, const urdf::Link* link, const srdf::Model& srdf_model);
635 
637  bool addJointModelGroup(const srdf::Model::Group& group);
638 
641  JointModel* constructJointModel(const urdf::Link* child_link, const srdf::Model& srdf_model);
642 
644  LinkModel* constructLinkModel(const urdf::Link* urdf_link);
645 
647  shapes::ShapePtr constructShape(const urdf::Geometry* geom);
648 };
649 } // namespace core
650 } // namespace moveit
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
size_t getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:222
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:145
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:72
const std::string & getName() const
The name of this link.
Definition: link_model.h:86
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
Definition: robot_model.h:106
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:515
const std::vector< std::string > & getLinkModelNames() const
Get the link names (of all links)
Definition: robot_model.h:287
std::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:422
std::string model_frame_
The reference (base) frame for this model. The frame is either extracted from the SRDF as a virtual j...
Definition: robot_model.h:485
const std::vector< const JointModel * > & getMultiDOFJointModels() const
This is a list of all multi-dof joints.
Definition: robot_model.h:194
std::vector< const JointModel * > single_dof_joints_
Definition: robot_model.h:549
std::vector< const JointModel * > joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:529
std::vector< std::string > link_model_names_with_collision_geometry_vector_
The vector of link names that corresponds to link_models_with_collision_geometry_vector_.
Definition: robot_model.h:512
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
Definition: robot_model.h:437
const std::vector< JointModel * > & getActiveJointModels()
Get the array of joints that are active (not fixed, not mimic) in this model.
Definition: robot_model.h:182
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:599
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState)....
Definition: robot_model.h:94
const std::vector< std::string > & getActiveJointModelNames() const
Get the array of active joint names, in the order they appear in the robot state.
Definition: robot_model.h:176
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:509
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:487
const std::vector< LinkModel * > & getLinkModels()
Get the array of links
Definition: robot_model.h:281
void computeFixedTransforms(const LinkModel *link, const Eigen::Isometry3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
const std::vector< const JointModelGroup * > & getJointModelGroups() const
Get the available joint groups.
Definition: robot_model.h:387
const std::vector< std::string > & getJointModelNames() const
Get the array of joint names, in the order they appear in the robot state.
Definition: robot_model.h:164
const std::vector< const JointModel * > & getJointModels() const
Get the array of joints, in the order they appear in the robot state.
Definition: robot_model.h:150
std::vector< const LinkModel * > link_model_vector_const_
The vector of links that are updated when computeTransforms() is called, in the order they are update...
Definition: robot_model.h:503
const std::vector< const JointModel * > & getMimicJointModels() const
Get the array of mimic joints, in the order they appear in the robot state.
Definition: robot_model.h:208
const JointModel * computeCommonRoot(const JointModel *a, const JointModel *b) const
Given two joints, find their common root.
const std::vector< const JointModelGroup * > & getEndEffectors() const
Get the map between end effector names and the groups they correspond to.
Definition: robot_model.h:414
bool isEmpty() const
Return true if the model is empty (has no root link, no joints)
Definition: robot_model.h:100
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:506
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for the joints is stored.
Definition: robot_model.h:560
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:593
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
std::size_t getLinkGeometryCount() const
Definition: robot_model.h:309
void interpolate(const double *from, const double *to, double t, double *state) const
std::vector< JointModel * > joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:526
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:547
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:77
static const LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link, const JointModelGroup *jmg=nullptr)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints.
const JointModel * getRootJoint() const
Get the root joint. There will be one root joint unless the model is empty. This is either extracted ...
Definition: robot_model.cpp:72
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
const std::vector< std::string > & getJointModelGroupNames() const
Get the names of all groups that are defined for this model.
Definition: robot_model.h:399
size_t getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints that form this state. Fixed joints have no DOF...
Definition: robot_model.h:431
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:62
const std::vector< std::string > & getLinkModelNamesWithCollisionGeometry() const
Get the names of the link models that have some collision geometry associated to themselves.
Definition: robot_model.h:299
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
const std::string & getRootJointName() const
Return the name of the root joint. Throws an exception if there is no root joint.
Definition: robot_model.h:131
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:579
std::vector< const JointModel * > active_joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:541
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints, in the order they appear in the robot state.
Definition: robot_model.h:201
std::size_t getLinkModelCount() const
Definition: robot_model.h:304
const JointModel * getJointOfVariable(const std::string &variable) const
Definition: robot_model.h:218
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:582
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:497
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
const std::vector< const LinkModel * > & getLinkModels() const
Get the array of links
Definition: robot_model.h:275
std::vector< std::string > active_joint_model_names_vector_
The vector of joint names that corresponds to active_joint_model_vector_.
Definition: robot_model.h:538
const std::vector< JointModelGroup * > & getJointModelGroups()
Get the available joint groups.
Definition: robot_model.h:393
const std::vector< JointModel * > & getJointModels()
Get the array of joints, in the order they appear in the robot state. This includes all types of join...
Definition: robot_model.h:158
std::vector< std::string > variable_names_
The names of the DOF that make up this state (this is just a sequence of joint variable names; not ne...
Definition: robot_model.h:566
const std::vector< const JointModel * > & getActiveJointModels() const
Get the array of joints that are active (not fixed, not mimic) in this model.
Definition: robot_model.h:170
const std::string & getRootLinkName() const
Get the name of the root link of the robot.
Definition: robot_model.h:238
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:576
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:489
const srdf::ModelConstSharedPtr & getSRDF() const
Get the parsed SRDF model.
Definition: robot_model.h:112
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:596
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for a RobotState.
Definition: robot_model.h:330
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
Definition: robot_model.h:443
std::vector< LinkModel * > link_model_vector_
The vector of links that are updated when computeTransforms() is called, in the order they are update...
Definition: robot_model.h:500
VariableIndexMap joint_variables_index_map_
The state includes all the joint variables that make up the joints the state consists of....
Definition: robot_model.h:574
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
const std::vector< const JointModel * > & getSingleDOFJointModels() const
This is a list of all single-dof joints (including mimic joints)
Definition: robot_model.h:188
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
Definition: robot_model.h:587
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:602
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
RobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
Construct a kinematic model from a parsed description and a list of planning groups.
Definition: robot_model.cpp:54
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:551
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:544
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:590
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
std::size_t getJointModelCount() const
Definition: robot_model.h:223
const std::vector< const LinkModel * > & getLinkModelsWithCollisionGeometry() const
Get the link models that have some collision geometry associated to themselves.
Definition: robot_model.h:293
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:532
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values) const
Compute the random values for a RobotState.
Definition: robot_model.h:323
double distance(const double *state1, const double *state2) const
Return the sum of joint distances between two states. An L1 norm. Only considers active joints.
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:213
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:523
std::size_t variable_count_
Get the number of variables necessary to describe this model.
Definition: robot_model.h:569
double getMaximumExtent() const
Definition: robot_model.h:354
std::string model_name_
The name of the robot.
Definition: robot_model.h:481
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:494
const LinkModel * getLinkModel(const std::string &link, bool *has_link=nullptr) const
Get a link by its name. Output error and return nullptr when the link is missing.
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:348
std::vector< JointModel * > active_joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
Definition: robot_model.h:535
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
const JointModel * getCommonRoot(const JointModel *a, const JointModel *b) const
Get the deepest joint in the kinematic tree that is a common parent of both joints passed as argument...
Definition: robot_model.h:455
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return nullptr when the joint is missing.
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:520
const std::string & getName() const
Get the model name.
Definition: robot_model.h:85
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:343
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
MOVEIT_CLASS_FORWARD(JointModelGroup)
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
Definition: link_model.h:68
std::map< std::string, LinkModel * > LinkModelMap
Map of names to instances for LinkModel.
Definition: link_model.h:58
std::vector< const JointModel::Bounds * > JointBoundsVector
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
std::map< std::string, JointModel * > JointModelMap
Map of names to instances for JointModel.
Definition: joint_model.h:99
Main namespace for MoveIt.
Definition: exceptions.h:43
a
Definition: plan.py:54
name
Definition: setup.py:7