moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
robot_model.hpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Ioan A. Sucan
5 * Copyright (c) 2008-2013, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Ioan Sucan */
37
38#pragma once
39
42#include <srdfdom/model.h>
43
44// joint types
51#include <rclcpp/logging.hpp>
52#include <Eigen/Geometry>
53#include <iostream>
54
56namespace moveit
57{
59namespace core
60{
61MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc
62
63static 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{
77public:
79 RobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model);
80
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
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 {
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
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;
462 }
463
465 void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators);
466
467protected:
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
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
604private:
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
#define MOVEIT_CLASS_FORWARD(C)
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
size_t getJointIndex() const
Get the index of this joint within the robot model.
const std::string & getName() const
Get the name of the joint.
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.
const std::string & getName() const
The name of this link.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
const std::vector< std::string > & getActiveJointModelNames() const
Get the array of active joint names, in the order they appear in the robot state.
std::size_t getVariableCount() const
Get the number of variables that describe this model.
std::string model_frame_
The reference (base) frame for this model. The frame is either extracted from the SRDF as a virtual j...
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
std::vector< const JointModel * > single_dof_joints_
std::vector< const JointModel * > joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state 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_.
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
const std::string & getRootLinkName() const
Get the name of the root link of the robot.
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
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_
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::string & getName() const
Get the model name.
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
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...
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...
const std::vector< const JointModel * > & getActiveJointModels() const
Get the array of joints that are active (not fixed, not mimic) in this model.
const std::vector< std::string > & getLinkModelNamesWithCollisionGeometry() const
Get the names of the link models that have some collision geometry associated to themselves.
bool isEmpty() const
Return true if the model is empty (has no root link, no joints)
const std::vector< const JointModelGroup * > & getJointModelGroups() const
Get the available joint groups.
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for the joints is stored.
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
std::size_t getLinkGeometryCount() const
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.
const std::vector< const JointModel * > & getMultiDOFJointModels() const
This is a list of all multi-dof joints.
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
const LinkModel * getRootLink() const
Get the physical root link of the robot.
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 std::vector< JointModelGroup * > & getJointModelGroups()
Get the available joint groups.
const JointModel * getRootJoint() const
Get the root joint. There will be one root joint unless the model is empty. This is either extracted ...
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
size_t getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
~RobotModel()
Destructor. Clear all memory.
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints, in the order they appear in the robot state.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
const std::vector< JointModel * > & getActiveJointModels()
Get the array of joints that are active (not fixed, not mimic) in this model.
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
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.
const std::vector< const LinkModel * > & getLinkModels() const
Get the array of links
const std::vector< const LinkModel * > & getLinkModelsWithCollisionGeometry() const
Get the link models that have some collision geometry associated to themselves.
const JointModel * getJointOfVariable(int variable_index) const
std::size_t getLinkModelCount() const
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
LinkModelMap link_model_map_
A map from link names to their instances.
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
std::vector< std::string > active_joint_model_names_vector_
The vector of joint names that corresponds to active_joint_model_vector_.
const std::vector< std::string > & getLinkModelNames() const
Get the link names (of all links)
const std::string & getRootJointName() const
Return the name of the root joint. Throws an exception if there is no root joint.
const std::vector< const JointModel * > & getSingleDOFJointModels() const
This is a list of all single-dof joints (including mimic joints)
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...
const std::vector< LinkModel * > & getLinkModels()
Get the array of links
std::vector< int > active_joint_model_start_index_
urdf::ModelInterfaceSharedPtr urdf_
const std::vector< std::string > & getJointModelGroupNames() const
Get the names of all groups that are defined for this model.
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for a RobotState.
const std::vector< const JointModel * > & getMimicJointModels() const
Get the array of mimic joints, in the order they appear in the robot state.
std::vector< LinkModel * > link_model_vector_
The vector of links that are updated when computeTransforms() is called, in the order they are update...
const std::vector< std::string > & getJointModelNames() const
Get the array of joint names, in the order they appear in the robot state.
VariableIndexMap joint_variables_index_map_
The state includes all the joint variables that make up the joints the state consists of....
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
std::vector< const JointModel * > multi_dof_joints_
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by 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...
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
JointModelGroupMap end_effectors_map_
The known end effectors.
const std::vector< const JointModelGroup * > & getEndEffectors() const
Get the map between end effector names and the groups they correspond to.
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
const srdf::ModelConstSharedPtr & getSRDF() const
Get the parsed SRDF model.
std::size_t getJointModelCount() const
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
const JointModel * computeCommonRoot(const JointModel *a, const JointModel *b) const
Given two joints, find their common root.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values) const
Compute the random values for a RobotState.
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.
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.
const JointModel * getJointOfVariable(const std::string &variable) const
std::size_t variable_count_
Get the number of variables necessary to describe this model.
double getMaximumExtent() const
std::string model_name_
The name of the robot.
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...
const LinkModel * root_link_
The first physical link for the robot.
const std::vector< const JointModel * > & getJointModels() const
Get the array of joints, in the order they appear in the robot state.
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
std::vector< JointModel * > active_joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState)....
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
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.
bool enforcePositionBounds(double *state) const
std::map< std::string, size_t > VariableIndexMap
Data type for holding mappings from variable names to their position in a state vector.
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.
std::map< std::string, LinkModel * > LinkModelMap
Map of names to instances for LinkModel.
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.
Main namespace for MoveIt.