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 #ifndef MOVEIT_CORE_ROBOT_MODEL_
39 #define MOVEIT_CORE_ROBOT_MODEL_
40 
43 #include <urdf/model.h>
44 #include <srdfdom/model.h>
45 
46 // joint types
53 
54 #include <Eigen/Geometry>
55 #include <iostream>
56 
58 namespace moveit
59 {
61 namespace core
62 {
63 MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc
64 
65 static inline void checkInterpolationParamBounds(const char LOGNAME[], double t)
66 {
67  if (std::isnan(t) || std::isinf(t))
68  {
69  throw Exception("Interpolation parameter is NaN or inf.");
70  }
71  ROS_WARN_STREAM_COND_NAMED(t < 0. || t > 1., LOGNAME, "Interpolation parameter is not in the range [0, 1]: " << t);
72 }
73 
77 {
78 public:
80  RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model);
81 
83  ~RobotModel();
84 
86  const std::string& getName() const
87  {
88  return model_name_;
89  }
90 
95  const std::string& getModelFrame() const
96  {
97  return model_frame_;
98  }
99 
101  bool isEmpty() const
102  {
103  return root_link_ == nullptr;
104  }
105 
107  const urdf::ModelInterfaceSharedPtr& getURDF() const
108  {
109  return urdf_;
110  }
111 
114  {
115  return srdf_;
116  }
117 
119  void printModelInfo(std::ostream& out) const;
120 
129  const JointModel* getRootJoint() const;
130 
132  const std::string& getRootJointName() const
133  {
134  return getRootJoint()->getName();
135  }
136 
138  bool hasJointModel(const std::string& name) const;
139 
141  const JointModel* getJointModel(const std::string& joint) const;
142 
144  const JointModel* getJointModel(int index) const;
145 
147  JointModel* getJointModel(const std::string& joint);
148 
151  const std::vector<const JointModel*>& getJointModels() const
152  {
154  }
155 
159  const std::vector<JointModel*>& getJointModels()
160  {
161  return joint_model_vector_;
162  }
163 
165  const std::vector<std::string>& getJointModelNames() const
166  {
168  }
169 
171  const std::vector<const JointModel*>& getActiveJointModels() const
172  {
174  }
175 
177  const std::vector<std::string>& getActiveJointModelNames() const
178  {
180  }
181 
183  const std::vector<JointModel*>& getActiveJointModels()
184  {
186  }
187 
189  const std::vector<const JointModel*>& getSingleDOFJointModels() const
190  {
191  return single_dof_joints_;
192  }
193 
195  const std::vector<const JointModel*>& getMultiDOFJointModels() const
196  {
197  return multi_dof_joints_;
198  }
199 
202  const std::vector<const JointModel*>& getContinuousJointModels() const
203  {
205  }
206 
209  const std::vector<const JointModel*>& getMimicJointModels() const
210  {
211  return mimic_joints_;
212  }
213 
214  const JointModel* getJointOfVariable(int variable_index) const
215  {
216  return joints_of_variable_[variable_index];
217  }
218 
219  const JointModel* getJointOfVariable(const std::string& variable) const
220  {
221  return joints_of_variable_[getVariableIndex(variable)];
222  }
223 
224  std::size_t getJointModelCount() const
225  {
226  return joint_model_vector_.size();
227  }
228 
236  const LinkModel* getRootLink() const;
237 
239  const std::string& getRootLinkName() const
240  {
241  return getRootLink()->getName();
242  }
243 
245  bool hasLinkModel(const std::string& name) const;
246 
248  const LinkModel* getLinkModel(const std::string& link) const;
249 
251  const LinkModel* getLinkModel(int index) const;
252 
254  LinkModel* getLinkModel(const std::string& link);
255 
268 
270  const std::vector<const LinkModel*>& getLinkModels() const
271  {
273  }
274 
276  const std::vector<LinkModel*>& getLinkModels()
277  {
278  return link_model_vector_;
279  }
280 
282  const std::vector<std::string>& getLinkModelNames() const
283  {
285  }
286 
288  const std::vector<const LinkModel*>& getLinkModelsWithCollisionGeometry() const
289  {
291  }
292 
294  const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
295  {
297  }
298 
299  std::size_t getLinkModelCount() const
300  {
301  return link_model_vector_.size();
302  }
303 
304  std::size_t getLinkGeometryCount() const
305  {
306  return link_geometry_count_;
307  }
308 
313 
315  void getVariableDefaultPositions(double* values) const;
316 
318  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
319  {
320  values.resize(variable_count_);
321  getVariableRandomPositions(rng, &values[0]);
322  }
323 
325  void getVariableDefaultPositions(std::vector<double>& values) const
326  {
327  values.resize(variable_count_);
328  getVariableDefaultPositions(&values[0]);
329  }
330 
333  std::map<std::string, double>& values) const;
334 
336  void getVariableDefaultPositions(std::map<std::string, double>& values) const;
337 
338  bool enforcePositionBounds(double* state) const
339  {
341  }
342  bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const;
343  bool satisfiesPositionBounds(const double* state, double margin = 0.0) const
344  {
346  }
347  bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
348  double margin = 0.0) const;
349  double getMaximumExtent() const
350  {
352  }
353  double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
354 
356  double distance(const double* state1, const double* state2) const;
357 
366  void interpolate(const double* from, const double* to, double t, double* state) const;
367 
373  bool hasJointModelGroup(const std::string& group) const;
374 
376  const JointModelGroup* getJointModelGroup(const std::string& name) const;
377 
379  JointModelGroup* getJointModelGroup(const std::string& name);
380 
382  const std::vector<const JointModelGroup*>& getJointModelGroups() const
383  {
385  }
386 
388  const std::vector<JointModelGroup*>& getJointModelGroups()
389  {
390  return joint_model_groups_;
391  }
392 
394  const std::vector<std::string>& getJointModelGroupNames() const
395  {
397  }
398 
400  bool hasEndEffector(const std::string& eef) const;
401 
403  const JointModelGroup* getEndEffector(const std::string& name) const;
404 
406  JointModelGroup* getEndEffector(const std::string& name);
407 
409  const std::vector<const JointModelGroup*>& getEndEffectors() const
410  {
411  return end_effectors_;
412  }
413 
417  std::size_t getVariableCount() const
418  {
419  return variable_count_;
420  }
421 
426  const std::vector<std::string>& getVariableNames() const
427  {
428  return variable_names_;
429  }
430 
432  const VariableBounds& getVariableBounds(const std::string& variable) const
433  {
434  return getJointOfVariable(variable)->getVariableBounds(variable);
435  }
436 
439  {
441  }
442 
443  void getMissingVariableNames(const std::vector<std::string>& variables,
444  std::vector<std::string>& missing_variables) const;
445 
447  int getVariableIndex(const std::string& variable) const;
448 
450  const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const
451  {
452  if (!a)
453  return b;
454  if (!b)
455  return a;
457  }
458 
460  void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators);
461 
462 protected:
464  void computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
465  LinkTransformMap& associated_transforms);
466 
468  const JointModel* computeCommonRoot(const JointModel* a, const JointModel* b) const;
469 
471  void updateMimicJoints(double* values) const;
472 
473  // GENERIC INFO
474 
476  std::string model_name_;
477 
480  std::string model_frame_;
481 
483 
484  urdf::ModelInterfaceSharedPtr urdf_;
485 
486  // LINKS
487 
490 
493 
495  std::vector<LinkModel*> link_model_vector_;
496 
498  std::vector<const LinkModel*> link_model_vector_const_;
499 
501  std::vector<std::string> link_model_names_vector_;
502 
504  std::vector<const LinkModel*> link_models_with_collision_geometry_vector_;
505 
508 
510  std::size_t link_geometry_count_;
511 
512  // JOINTS
513 
516 
519 
521  std::vector<JointModel*> joint_model_vector_;
522 
524  std::vector<const JointModel*> joint_model_vector_const_;
525 
527  std::vector<std::string> joint_model_names_vector_;
528 
530  std::vector<JointModel*> active_joint_model_vector_;
531 
533  std::vector<std::string> active_joint_model_names_vector_;
534 
536  std::vector<const JointModel*> active_joint_model_vector_const_;
537 
539  std::vector<const JointModel*> continuous_joint_model_vector_;
540 
542  std::vector<const JointModel*> mimic_joints_;
543 
544  std::vector<const JointModel*> single_dof_joints_;
545 
546  std::vector<const JointModel*> multi_dof_joints_;
547 
555  std::vector<int> common_joint_roots_;
556 
557  // INDEXING
558 
561  std::vector<std::string> variable_names_;
562 
564  std::size_t variable_count_;
565 
570 
572 
575 
577  std::vector<const JointModel*> joints_of_variable_;
578 
579  // GROUPS
580 
583 
586 
588  std::vector<JointModelGroup*> joint_model_groups_;
589 
591  std::vector<const JointModelGroup*> joint_model_groups_const_;
592 
594  std::vector<std::string> joint_model_group_names_;
595 
597  std::vector<const JointModelGroup*> end_effectors_;
598 
600  void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model);
601 
603  void buildGroups(const srdf::Model& srdf_model);
604 
606  void buildGroupsInfoSubgroups(const srdf::Model& srdf_model);
607 
609  void buildGroupsInfoEndEffectors(const srdf::Model& srdf_model);
610 
612  void buildMimic(const urdf::ModelInterface& urdf_model);
613 
615  void buildGroupStates(const srdf::Model& srdf_model);
616 
618  void buildJointInfo();
619 
621  void computeDescendants();
622 
624  void computeCommonRoots();
625 
628  JointModel* buildRecursive(LinkModel* parent, const urdf::Link* link, const srdf::Model& srdf_model);
629 
631  bool addJointModelGroup(const srdf::Model::Group& group);
632 
635  JointModel* constructJointModel(const urdf::Joint* urdf_joint_model, const urdf::Link* child_link,
636  const srdf::Model& srdf_model);
637 
639  LinkModel* constructLinkModel(const urdf::Link* urdf_link);
640 
642  shapes::ShapePtr constructShape(const urdf::Geometry* geom);
643 };
644 } // namespace core
645 } // namespace moveit
646 
647 namespace robot_model = moveit::core;
648 namespace robot_state = moveit::core;
649 
650 #endif
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:542
const std::vector< const JointModelGroup * > & getEndEffectors() const
Get the map between end effector names and the groups they correspond to.
Definition: robot_model.h:409
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
const std::vector< std::string > & getJointModelGroupNames() const
Get the names of all groups that are defined for this model.
Definition: robot_model.h:394
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:343
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:533
Core components of MoveIt!
const std::vector< std::string > & getLinkModelNames() const
Get the link names (of all links)
Definition: robot_model.h:282
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:495
JointModel * constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model)
Given a urdf joint model, a child link and a set of virtual joints, build up the corresponding JointM...
#define ROS_WARN_STREAM_COND_NAMED(cond, name, args)
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:577
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:501
void computeFixedTransforms(const LinkModel *link, const Eigen::Isometry3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
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:69
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:214
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:498
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:202
const std::vector< const JointModel * > & getMultiDOFJointModels() const
This is a list of all multi-dof joints.
Definition: robot_model.h:195
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:294
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:171
void buildGroupsInfoSubgroups(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:574
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints...
std::vector< double > values
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:492
std::size_t getLinkModelCount() const
Definition: robot_model.h:299
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
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:450
const std::vector< const JointModelGroup * > & getJointModelGroups() const
Get the available joint groups.
Definition: robot_model.h:382
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:57
const std::vector< const JointModel * > & getSingleDOFJointModels() const
This is a list of all single-dof joints (including mimic joints)
Definition: robot_model.h:189
const std::vector< JointModel * > & getActiveJointModels()
Get the array of joints that are active (not fixed, not mimic) in this model.
Definition: robot_model.h:183
void buildJointInfo()
Compute helpful information about joints.
void buildGroupsInfoEndEffectors(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
int getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
const JointModel * computeCommonRoot(const JointModel *a, const JointModel *b) const
Given two joints, find their common root.
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:489
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values) const
Compute the random values for a RobotState.
Definition: robot_model.h:318
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:585
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
double distance(const double *state1, const double *state2) const
Return the sum of joint distances between two states. Only considers active joints.
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:65
geometry_msgs::TransformStamped t
std::shared_ptr< Shape > ShapePtr
const std::vector< LinkModel * > & getLinkModels()
Get the array of links.
Definition: robot_model.h:276
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:209
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
Definition: robot_model.h:438
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return NULL when the joint is missing.
void buildGroups(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build up the groups in this kinematic model.
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
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:177
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:80
void buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model)
Given an URDF model and a SRDF model, build a full kinematic model.
Definition: robot_model.cpp:85
const std::string & getRootLinkName() const
Get the name of the root link of the robot.
Definition: robot_model.h:239
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
std::string model_name_
The name of the robot.
Definition: robot_model.h:476
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
LinkModel * constructLinkModel(const urdf::Link *urdf_link)
Given a urdf link, build the corresponding LinkModel object.
shapes::ShapePtr constructShape(const urdf::Geometry *geom)
Given a geometry spec from the URDF and a filename (for a mesh), construct the corresponding shape ob...
unsigned int index
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:569
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
void computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
std::size_t getJointModelCount() const
Definition: robot_model.h:224
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:75
std::shared_ptr< const Model > ModelConstSharedPtr
MOVEIT_CLASS_FORWARD(RobotModel)
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:132
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
Definition: robot_model.h:582
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
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:159
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
std::map< std::string, JointModel * > JointModelMap
Map of names to instances for JointModel.
Definition: joint_model.h:91
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState). This frame depends on the root joint. As such, the frame is either extracted from SRDF, or it is assumed to be the name of the root link.
Definition: robot_model.h:95
std::vector< const JointModel::Bounds * > JointBoundsVector
std::size_t variable_count_
Get the number of variables necessary to describe this model.
Definition: robot_model.h:564
This may be thrown if unrecoverable errors occur.
Definition: exceptions.h:53
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:515
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:524
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for a RobotState.
Definition: robot_model.h:325
static void checkInterpolationParamBounds(const char LOGNAME[], double t)
Definition: robot_model.h:65
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:151
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
Definition: robot_model.h:107
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:482
std::map< std::string, int > VariableIndexMap
Data type for holding mappings from variable names to their position in a state vector.
Definition: joint_model.h:82
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:591
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:539
std::vector< const JointModel * > single_dof_joints_
Definition: robot_model.h:544
const std::string LOGNAME
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:597
std::size_t getLinkGeometryCount() const
Definition: robot_model.h:304
std::map< std::string, LinkModel * > LinkModelMap
Map of names to instances for LinkModel.
Definition: link_model.h:58
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
const std::vector< const LinkModel * > & getLinkModels() const
Get the array of links.
Definition: robot_model.h:270
void computeCommonRoots()
For every pair of joints, pre-compute the common roots of the joints.
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:165
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:530
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:510
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:518
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:527
const JointModel * getJointOfVariable(const std::string &variable) const
Definition: robot_model.h:219
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:484
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:480
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:521
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:561
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:426
bool addJointModelGroup(const srdf::Model::Group &group)
Construct a JointModelGroup given a SRDF description group.
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:571
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:546
std::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:417
void buildMimic(const urdf::ModelInterface &urdf_model)
Given the URDF model, build up the mimic joints (mutually constrained joints)
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:338
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:588
const srdf::ModelConstSharedPtr & getSRDF() const
Get the parsed SRDF model.
Definition: robot_model.h:113
Main namespace for MoveIt!
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:594
void buildGroupStates(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build the default states defined in the SRDF...
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:536
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:504
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for thw joints is stored.
Definition: robot_model.h:555
JointModel * buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model)
(This function is mostly intended for internal use). Given a parent link, build up (recursively)...
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:432
const std::vector< JointModelGroup * > & getJointModelGroups()
Get the available joint groups.
Definition: robot_model.h:388
bool isEmpty() const
Return true if the model is empty (has no root link, no joints)
Definition: robot_model.h:101
const std::vector< const LinkModel * > & getLinkModelsWithCollisionGeometry() const
Get the link models that have some collision geometry associated to themselves.
Definition: robot_model.h:288
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:507
double getMaximumExtent() const
Definition: robot_model.h:349
const std::string & getName() const
Get the model name.
Definition: robot_model.h:86
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:131


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