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 
43 
44 #include <urdf/model.h>
45 #include <srdfdom/model.h>
46 
47 // joint types
54 
55 #include <Eigen/Geometry>
56 #include <iostream>
57 
59 namespace moveit
60 {
62 namespace core
63 {
64 MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc
65 
66 static inline void checkInterpolationParamBounds(const char LOGNAME[], double t)
67 {
68  if (std::isnan(t) || std::isinf(t))
69  {
70  throw Exception("Interpolation parameter is NaN or inf.");
71  }
72  ROS_WARN_STREAM_COND_NAMED(t < 0. || t > 1., LOGNAME, "Interpolation parameter is not in the range [0, 1]: " << t);
73 }
74 
77 class RobotModel
78 {
79 public:
81  RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model);
82 
84  ~RobotModel();
85 
87  const std::string& getName() const
88  {
89  return model_name_;
90  }
91 
96  const std::string& getModelFrame() const
97  {
98  return model_frame_;
99  }
100 
102  bool isEmpty() const
103  {
104  return root_link_ == nullptr;
105  }
106 
108  const urdf::ModelInterfaceSharedPtr& getURDF() const
109  {
110  return urdf_;
111  }
112 
114  const srdf::ModelConstSharedPtr& getSRDF() const
115  {
116  return srdf_;
117  }
118 
120  void printModelInfo(std::ostream& out) const;
121 
130  const JointModel* getRootJoint() const;
131 
133  const std::string& getRootJointName() const
134  {
135  return getRootJoint()->getName();
136  }
137 
139  bool hasJointModel(const std::string& name) const;
140 
142  const JointModel* getJointModel(const std::string& joint) const;
143 
145  const JointModel* getJointModel(int index) const;
146 
148  JointModel* getJointModel(const std::string& joint);
149 
152  const std::vector<const JointModel*>& getJointModels() const
153  {
155  }
156 
160  const std::vector<JointModel*>& getJointModels()
161  {
163  }
164 
166  const std::vector<std::string>& getJointModelNames() const
167  {
169  }
170 
172  const std::vector<const JointModel*>& getActiveJointModels() const
173  {
175  }
176 
178  const std::vector<std::string>& getActiveJointModelNames() const
179  {
181  }
182 
184  const std::vector<JointModel*>& getActiveJointModels()
185  {
187  }
188 
190  const std::vector<const JointModel*>& getSingleDOFJointModels() const
191  {
192  return single_dof_joints_;
193  }
194 
196  const std::vector<const JointModel*>& getMultiDOFJointModels() const
197  {
198  return multi_dof_joints_;
199  }
200 
203  const std::vector<const JointModel*>& getContinuousJointModels() const
204  {
206  }
207 
210  const std::vector<const JointModel*>& getMimicJointModels() const
211  {
212  return mimic_joints_;
213  }
214 
215  const JointModel* getJointOfVariable(int variable_index) const
216  {
217  return joints_of_variable_[variable_index];
218  }
219 
220  const JointModel* getJointOfVariable(const std::string& variable) const
221  {
222  return joints_of_variable_[getVariableIndex(variable)];
223  }
224 
225  std::size_t getJointModelCount() const
226  {
227  return joint_model_vector_.size();
228  }
229 
237  const LinkModel* getRootLink() const;
238 
240  const std::string& getRootLinkName() const
241  {
242  return getRootLink()->getName();
243  }
244 
248  bool hasLinkModel(const std::string& name) const;
249 
251  const LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr) const;
252 
254  const LinkModel* getLinkModel(int index) const;
255 
257  LinkModel* getLinkModel(const std::string& link, bool* has_link = nullptr);
258 
274  Eigen::Isometry3d& transform,
275  const JointModelGroup* jmg = nullptr);
277  const JointModelGroup* jmg = nullptr)
278  {
279  Eigen::Isometry3d unused;
280  return getRigidlyConnectedParentLinkModel(link, unused, jmg);
281  }
282 
284  const std::vector<const LinkModel*>& getLinkModels() const
285  {
287  }
288 
290  const std::vector<LinkModel*>& getLinkModels()
291  {
292  return link_model_vector_;
293  }
294 
296  const std::vector<std::string>& getLinkModelNames() const
297  {
299  }
300 
302  const std::vector<const LinkModel*>& getLinkModelsWithCollisionGeometry() const
303  {
305  }
306 
308  const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
309  {
311  }
312 
313  std::size_t getLinkModelCount() const
314  {
315  return link_model_vector_.size();
316  }
317 
318  std::size_t getLinkGeometryCount() const
319  {
320  return link_geometry_count_;
321  }
322 
326  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const;
327 
329  void getVariableDefaultPositions(double* values) const;
330 
332  void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
333  {
334  values.resize(variable_count_);
335  getVariableRandomPositions(rng, &values[0]);
336  }
337 
339  void getVariableDefaultPositions(std::vector<double>& values) const
340  {
341  values.resize(variable_count_);
343  }
344 
347  std::map<std::string, double>& values) const;
348 
350  void getVariableDefaultPositions(std::map<std::string, double>& values) const;
351 
352  bool enforcePositionBounds(double* state) const
353  {
355  }
356  bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const;
357  bool satisfiesPositionBounds(const double* state, double margin = 0.0) const
358  {
360  }
361  bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
362  double margin = 0.0) const;
363  double getMaximumExtent() const
364  {
366  }
367  double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
368 
370  double distance(const double* state1, const double* state2) const;
371 
380  void interpolate(const double* from, const double* to, double t, double* state) const;
381 
387  bool hasJointModelGroup(const std::string& group) const;
388 
390  const JointModelGroup* getJointModelGroup(const std::string& name) const;
391 
393  JointModelGroup* getJointModelGroup(const std::string& name);
394 
396  const std::vector<const JointModelGroup*>& getJointModelGroups() const
397  {
399  }
400 
402  const std::vector<JointModelGroup*>& getJointModelGroups()
403  {
404  return joint_model_groups_;
405  }
406 
408  const std::vector<std::string>& getJointModelGroupNames() const
409  {
411  }
412 
414  bool hasEndEffector(const std::string& eef) const;
415 
417  const JointModelGroup* getEndEffector(const std::string& name) const;
418 
420  JointModelGroup* getEndEffector(const std::string& name);
421 
423  const std::vector<const JointModelGroup*>& getEndEffectors() const
424  {
425  return end_effectors_;
426  }
427 
431  std::size_t getVariableCount() const
432  {
433  return variable_count_;
434  }
435 
440  const std::vector<std::string>& getVariableNames() const
441  {
442  return variable_names_;
443  }
444 
446  const VariableBounds& getVariableBounds(const std::string& variable) const
447  {
448  return getJointOfVariable(variable)->getVariableBounds(variable);
449  }
450 
453  {
455  }
456 
457  void getMissingVariableNames(const std::vector<std::string>& variables,
458  std::vector<std::string>& missing_variables) const;
459 
461  int getVariableIndex(const std::string& variable) const;
462 
464  const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const
465  {
466  if (!a)
467  return b;
468  if (!b)
469  return a;
471  }
472 
474  void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators);
475 
476 protected:
478  void computeFixedTransforms(const LinkModel* link, const Eigen::Isometry3d& transform,
479  LinkTransformMap& associated_transforms);
480 
482  const JointModel* computeCommonRoot(const JointModel* a, const JointModel* b) const;
483 
485  void updateMimicJoints(double* values) const;
486 
487  // GENERIC INFO
488 
490  std::string model_name_;
491 
494  std::string model_frame_;
495 
497 
498  urdf::ModelInterfaceSharedPtr urdf_;
499 
500  // LINKS
501 
503  const LinkModel* root_link_;
504 
507 
509  std::vector<LinkModel*> link_model_vector_;
510 
512  std::vector<const LinkModel*> link_model_vector_const_;
513 
515  std::vector<std::string> link_model_names_vector_;
516 
518  std::vector<const LinkModel*> link_models_with_collision_geometry_vector_;
519 
521  std::vector<std::string> link_model_names_with_collision_geometry_vector_;
522 
524  std::size_t link_geometry_count_;
525 
526  // JOINTS
527 
529  const JointModel* root_joint_;
530 
533 
535  std::vector<JointModel*> joint_model_vector_;
536 
538  std::vector<const JointModel*> joint_model_vector_const_;
539 
541  std::vector<std::string> joint_model_names_vector_;
542 
544  std::vector<JointModel*> active_joint_model_vector_;
545 
547  std::vector<std::string> active_joint_model_names_vector_;
548 
550  std::vector<const JointModel*> active_joint_model_vector_const_;
551 
553  std::vector<const JointModel*> continuous_joint_model_vector_;
554 
556  std::vector<const JointModel*> mimic_joints_;
557 
558  std::vector<const JointModel*> single_dof_joints_;
559 
560  std::vector<const JointModel*> multi_dof_joints_;
561 
569  std::vector<int> common_joint_roots_;
570 
571  // INDEXING
572 
575  std::vector<std::string> variable_names_;
576 
578  std::size_t variable_count_;
579 
584 
585  std::vector<int> active_joint_model_start_index_;
586 
589 
591  std::vector<const JointModel*> joints_of_variable_;
592 
593  // GROUPS
594 
597 
600 
602  std::vector<JointModelGroup*> joint_model_groups_;
603 
605  std::vector<const JointModelGroup*> joint_model_groups_const_;
606 
608  std::vector<std::string> joint_model_group_names_;
609 
611  std::vector<const JointModelGroup*> end_effectors_;
612 
614  void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model);
615 
617  void buildGroups(const srdf::Model& srdf_model);
618 
621 
623  void buildGroupsInfoEndEffectors(const srdf::Model& srdf_model);
624 
626  void buildMimic(const urdf::ModelInterface& urdf_model);
627 
629  void buildGroupStates(const srdf::Model& srdf_model);
630 
632  void buildJointInfo();
633 
636 
638  void computeCommonRoots();
639 
642  JointModel* buildRecursive(LinkModel* parent, const urdf::Link* link, const srdf::Model& srdf_model);
643 
645  bool addJointModelGroup(const srdf::Model::Group& group);
646 
649  JointModel* constructJointModel(const urdf::Joint* urdf_joint_model, const urdf::Link* child_link,
650  const srdf::Model& srdf_model);
651 
653  LinkModel* constructLinkModel(const urdf::Link* urdf_link);
654 
656  shapes::ShapePtr constructShape(const urdf::Geometry* geom);
657 };
658 } // namespace core
659 } // namespace moveit
660 
661 namespace robot_model = moveit::core;
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
Core components of MoveIt.
Definition: kinematics_base.h:83
moveit::core::RobotModel::getModelFrame
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:162
lexical_casts.h
locale-agnostic conversion functions from floating point numbers to strings
moveit::core::RobotModel::variable_count_
std::size_t variable_count_
Get the number of variables necessary to describe this model.
Definition: robot_model.h:644
moveit::core::RobotModel::getLinkModels
const std::vector< const LinkModel * > & getLinkModels() const
Get the array of links
Definition: robot_model.h:350
moveit::core::RobotModel::getJointModel
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return NULL when the joint is missing.
Definition: robot_model.cpp:1286
moveit::core::RobotModel::getActiveJointModelNames
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:244
moveit::core::RobotModel::end_effectors_
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
Definition: robot_model.h:677
exceptions.h
fixed_joint_model.h
moveit::core::RobotModel::getSRDF
const srdf::ModelConstSharedPtr & getSRDF() const
Get the parsed SRDF model.
Definition: robot_model.h:180
moveit::core::RobotModel::active_joint_model_vector_
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:610
moveit::core::RobotModel::active_joint_model_vector_const_
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:616
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::RobotModel::getURDF
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
Definition: robot_model.h:174
moveit::core::RobotModel::setKinematicsAllocators
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
Definition: robot_model.cpp:1498
moveit::core::RobotModel::constructLinkModel
LinkModel * constructLinkModel(const urdf::Link *urdf_link)
Given a urdf link, build the corresponding LinkModel object.
Definition: robot_model.cpp:1161
moveit::core::RobotModel::joint_model_vector_const_
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:604
moveit::core::RobotModel::addJointModelGroup
bool addJointModelGroup(const srdf::Model::Group &group)
Construct a JointModelGroup given a SRDF description group.
Definition: robot_model.cpp:733
moveit::core::RobotModel::joint_model_groups_const_
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:671
moveit::core::RobotModel::buildGroupsInfoSubgroups
void buildGroupsInfoSubgroups()
Compute helpful information about groups (that can be queried later)
Definition: robot_model.cpp:621
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::RobotModel::getRigidlyConnectedParentLinkModel
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link, Eigen::Isometry3d &transform, const JointModelGroup *jmg=nullptr)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints.
Definition: robot_model.cpp:1346
moveit::core::JointModel::getName
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:196
moveit::core::RobotModel::link_geometry_count_
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
Definition: robot_model.h:590
moveit::core::RobotModel::computeDescendants
void computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
Definition: robot_model.cpp:295
moveit::core::RobotModel::joint_model_names_vector_
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
Definition: robot_model.h:607
moveit::core::RobotModel::getVariableBounds
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:512
moveit::core::RobotModel::continuous_joint_model_vector_
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
Definition: robot_model.h:619
moveit::core::RobotModel::getRootJoint
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:141
moveit::core::RobotModel::joint_model_vector_
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:601
moveit::core::RobotModel::joint_model_group_map_
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
Definition: robot_model.h:662
moveit::core::JointBoundsVector
std::vector< const JointModel::Bounds * > JointBoundsVector
Definition: joint_model_group.h:132
moveit::core::RobotModel::getLinkGeometryCount
std::size_t getLinkGeometryCount() const
Definition: robot_model.h:384
class_forward.h
moveit::core::RobotModel::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that describe this model.
Definition: robot_model.h:497
moveit::core::RobotModel::buildRecursive
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),...
Definition: robot_model.cpp:849
moveit::core::RobotModel::computeCommonRoots
void computeCommonRoots()
For every pair of joints, pre-compute the common roots of the joints.
Definition: robot_model.cpp:264
moveit::core::RobotModel::joint_model_group_names_
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
Definition: robot_model.h:674
moveit::core::RobotModel::joint_model_groups_
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
Definition: robot_model.h:668
moveit::core::RobotModel::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
Definition: robot_model.cpp:547
moveit::core::RobotModel::buildMimic
void buildMimic(const urdf::ModelInterface &urdf_model)
Given the URDF model, build up the mimic joints (mutually constrained joints)
Definition: robot_model.cpp:450
moveit::core::RobotModel::interpolate
void interpolate(const double *from, const double *to, double t, double *state) const
Definition: robot_model.cpp:1486
moveit::core::RobotModel::joint_model_map_
JointModelMap joint_model_map_
A map from joint names to their instances.
Definition: robot_model.h:598
moveit::core::RobotModel::single_dof_joints_
std::vector< const JointModel * > single_dof_joints_
Definition: robot_model.h:624
planar_joint_model.h
moveit::core::RobotModel::link_model_map_
LinkModelMap link_model_map_
A map from link names to their instances.
Definition: robot_model.h:572
moveit::core::RobotModel::link_model_vector_const_
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:578
moveit::core::RobotModel::srdf_
srdf::ModelConstSharedPtr srdf_
Definition: robot_model.h:562
moveit::core::RobotModel::link_model_vector_
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:575
moveit::core::RobotModel::getEndEffectors
const std::vector< const JointModelGroup * > & getEndEffectors() const
Get the map between end effector names and the groups they correspond to.
Definition: robot_model.h:489
moveit::core::RobotModel::getMaximumExtent
double getMaximumExtent() const
Definition: robot_model.h:429
moveit::core::RobotModel::hasEndEffector
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
Definition: robot_model.cpp:509
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
moveit::core::JointModelMap
std::map< std::string, JointModel * > JointModelMap
Map of names to instances for JointModel.
Definition: joint_model.h:156
moveit::core::RobotModel::link_models_with_collision_geometry_vector_
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
Definition: robot_model.h:584
moveit::core::RobotModel::common_joint_roots_
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:635
moveit::core::RobotModel::getLinkModelsWithCollisionGeometry
const std::vector< const LinkModel * > & getLinkModelsWithCollisionGeometry() const
Get the link models that have some collision geometry associated to themselves.
Definition: robot_model.h:368
moveit::core::RobotModel::isEmpty
bool isEmpty() const
Return true if the model is empty (has no root link, no joints)
Definition: robot_model.h:168
moveit::core::JointModel::getVariableBounds
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
Definition: joint_model.cpp:213
moveit::core::RobotModel::mimic_joints_
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
Definition: robot_model.h:622
moveit::core::RobotModel::getJointModelGroupNames
const std::vector< std::string > & getJointModelGroupNames() const
Get the names of all groups that are defined for this model.
Definition: robot_model.h:474
moveit::core::RobotModel::getVariableNames
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:506
moveit::core::RobotModel::model_frame_
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:560
moveit::core::RobotModel::multi_dof_joints_
std::vector< const JointModel * > multi_dof_joints_
Definition: robot_model.h:626
moveit::core::RobotModel::root_link_
const LinkModel * root_link_
The first physical link for the robot.
Definition: robot_model.h:569
moveit::core::RobotModel::link_model_names_vector_
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
Definition: robot_model.h:581
moveit::core::RobotModel::urdf_
urdf::ModelInterfaceSharedPtr urdf_
Definition: robot_model.h:564
moveit::core::RobotModel::RobotModel
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:123
moveit::core::RobotModel::getJointOfVariable
const JointModel * getJointOfVariable(int variable_index) const
Definition: robot_model.h:281
model.h
moveit::core::RobotModel::buildGroupsInfoEndEffectors
void buildGroupsInfoEndEffectors(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
Definition: robot_model.cpp:650
srdf::ModelConstSharedPtr
std::shared_ptr< const Model > ModelConstSharedPtr
moveit::core::RobotModel::computeFixedTransforms
void computeFixedTransforms(const LinkModel *link, const Eigen::Isometry3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
Definition: robot_model.cpp:1619
moveit::core::RobotModel::hasJointModelGroup
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
Definition: robot_model.cpp:542
moveit::core::RobotModel::joints_of_variable_
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
Definition: robot_model.h:657
moveit::core::RobotModel::getActiveJointModels
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:238
moveit::core::RobotModel::satisfiesPositionBounds
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
Definition: robot_model.h:423
moveit::core::RobotModel::active_joint_models_bounds_
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
Definition: robot_model.h:654
moveit::core::RobotModel::getLinkModelCount
std::size_t getLinkModelCount() const
Definition: robot_model.h:379
moveit::core::RobotModel::enforcePositionBounds
bool enforcePositionBounds(double *state) const
Definition: robot_model.h:418
moveit::core::RobotModel::getContinuousJointModels
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:269
moveit::core::RobotModel::constructJointModel
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...
Definition: robot_model.cpp:946
moveit::core::JointModel::getJointIndex
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:279
moveit::core::RobotModel::link_model_names_with_collision_geometry_vector_
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:587
moveit::core::RobotModel::variable_names_
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:641
moveit::core::LOGNAME
const std::string LOGNAME
Definition: joint_model_group.cpp:165
moveit::core::RobotModel::buildGroupStates
void buildGroupStates(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build the default states defined in the SRDF.
Definition: robot_model.cpp:390
moveit::core::RobotModel::getRootLink
const LinkModel * getRootLink() const
Get the physical root link of the robot.
Definition: robot_model.cpp:146
moveit::core::RobotModel::buildJointInfo
void buildJointInfo()
Compute helpful information about joints.
Definition: robot_model.cpp:315
moveit::core::LinkTransformMap
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
moveit::core::RobotModel::active_joint_model_start_index_
std::vector< int > active_joint_model_start_index_
Definition: robot_model.h:651
moveit::core::RobotModel::getName
const std::string & getName() const
Get the model name.
Definition: robot_model.h:153
ROS_WARN_STREAM_COND_NAMED
#define ROS_WARN_STREAM_COND_NAMED(cond, name, args)
moveit::core::LinkModelMap
std::map< std::string, LinkModel * > LinkModelMap
Map of names to instances for LinkModel.
Definition: link_model.h:58
revolute_joint_model.h
random_numbers::RandomNumberGenerator
moveit::core::RobotModel::printModelInfo
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
Definition: robot_model.cpp:1570
moveit::core::RobotModel::getLinkModel
const LinkModel * getLinkModel(const std::string &link, bool *has_link=nullptr) const
Get a link by its name. Output error and return NULL when the link is missing.
Definition: robot_model.cpp:1315
moveit::core::RobotModel::getMissingVariableNames
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
Definition: robot_model.cpp:1424
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
moveit::core::RobotModel::getCommonRoot
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:530
moveit::core::RobotModel::model_name_
std::string model_name_
The name of the robot.
Definition: robot_model.h:556
moveit::core::RobotModel::getJointModelGroups
const std::vector< const JointModelGroup * > & getJointModelGroups() const
Get the available joint groups.
Definition: robot_model.h:462
moveit::core::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(JointModelGroup)
moveit::core::RobotModel::getRootLinkName
const std::string & getRootLinkName() const
Get the name of the root link of the robot.
Definition: robot_model.h:306
moveit::core::RobotModel::getMimicJointModels
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:276
moveit::core::RobotModel::active_joint_model_names_vector_
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:613
moveit::core::RobotModel::getVariableIndex
int getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
Definition: robot_model.cpp:1435
moveit::core::RobotModel::getActiveJointModelsBounds
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
Definition: robot_model.h:518
moveit::core::RobotModel::distance
double distance(const double *state1, const double *state2) const
Return the sum of joint distances between two states. Only considers active joints.
Definition: robot_model.cpp:1476
moveit::core::RobotModel::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
Get the link names (of all links)
Definition: robot_model.h:362
moveit::core::RobotModel::getJointModels
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:218
joint_model_group.h
moveit::core::JointModelGroupMap
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
Definition: joint_model_group.h:127
moveit::core::RobotModel::hasJointModel
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
Definition: robot_model.cpp:1276
moveit::core::RobotModel::constructShape
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...
Definition: robot_model.cpp:1237
floating_joint_model.h
moveit::core::RobotModel::joint_variables_index_map_
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:649
srdf::Model::Group
moveit::core::RobotModel::getMultiDOFJointModels
const std::vector< const JointModel * > & getMultiDOFJointModels() const
This is a list of all multi-dof joints.
Definition: robot_model.h:262
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:80
moveit::core::RobotModel::getJointModelCount
std::size_t getJointModelCount() const
Definition: robot_model.h:291
moveit::core::RobotModel::getLinkModelNamesWithCollisionGeometry
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:374
moveit::core::RobotModel::root_joint_
const JointModel * root_joint_
The root joint.
Definition: robot_model.h:595
moveit::core::RobotModel::buildGroups
void buildGroups(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build up the groups in this kinematic model.
Definition: robot_model.cpp:569
moveit::core::RobotModel::getVariableDefaultPositions
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
Definition: robot_model.cpp:1408
moveit::core::RobotModel::getSingleDOFJointModels
const std::vector< const JointModel * > & getSingleDOFJointModels() const
This is a list of all single-dof joints (including mimic joints)
Definition: robot_model.h:256
moveit::core::checkInterpolationParamBounds
static void checkInterpolationParamBounds(const char LOGNAME[], double t)
Definition: robot_model.h:132
srdf::Model
moveit::core::RobotModel::getVariableRandomPositions
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
Definition: robot_model.cpp:1391
moveit::core::RobotModel::updateMimicJoints
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
Definition: robot_model.cpp:1381
moveit::core::RobotModel::~RobotModel
~RobotModel()
Destructor. Clear all memory.
Definition: robot_model.cpp:131
moveit::core::RobotModel::computeCommonRoot
const JointModel * computeCommonRoot(const JointModel *a, const JointModel *b) const
Given two joints, find their common root.
moveit::core::RobotModel::buildModel
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:151
moveit::core::RobotModel::getJointModelNames
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:232
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::RobotModel::getRootJointName
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:199
moveit::core::RobotModel::hasLinkModel
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
Definition: robot_model.cpp:1281
moveit::core::RobotModel::getEndEffector
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
Definition: robot_model.cpp:514
moveit::core::RobotModel::end_effectors_map_
JointModelGroupMap end_effectors_map_
The known end effectors.
Definition: robot_model.h:665
prismatic_joint_model.h


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