42#include <srdfdom/model.h>
51#include <rclcpp/logging.hpp>
52#include <Eigen/Geometry>
63static inline void checkInterpolationParamBounds(
const rclcpp::Logger& logger,
double t)
65 if (std::isnan(t) || std::isinf(t))
67 throw Exception(
"Interpolation parameter is NaN or inf.");
70 RCLCPP_WARN_STREAM_EXPRESSION(logger, t < 0. || t > 1.,
"Interpolation parameter is not in the range [0, 1]: " << t);
79 RobotModel(
const urdf::ModelInterfaceSharedPtr& urdf_model,
const srdf::ModelConstSharedPtr& srdf_model);
106 const urdf::ModelInterfaceSharedPtr&
getURDF()
const
112 const srdf::ModelConstSharedPtr&
getSRDF()
const
338 std::map<std::string, double>& values)
const;
353 double margin = 0.0)
const;
361 double distance(
const double* state1,
const double* state2)
const;
371 void interpolate(
const double* from,
const double* to,
double t,
double* state)
const;
449 std::vector<std::string>& missing_variables)
const;
489 urdf::ModelInterfaceSharedPtr
urdf_;
606 void buildModel(
const urdf::ModelInterface& urdf_model,
const srdf::Model& srdf_model);
609 void buildGroups(
const srdf::Model& srdf_model);
612 void buildGroupsInfoSubgroups();
615 void buildGroupsInfoEndEffectors(
const srdf::Model& srdf_model);
618 void buildMimic(
const urdf::ModelInterface& urdf_model);
621 void buildGroupStates(
const srdf::Model& srdf_model);
624 void buildJointInfo();
627 void computeDescendants();
630 void computeCommonRoots();
634 JointModel* buildRecursive(
LinkModel* parent,
const urdf::Link* link,
const srdf::Model& srdf_model);
637 bool addJointModelGroup(
const srdf::Model::Group& group);
641 JointModel* constructJointModel(
const urdf::Link* child_link,
const srdf::Model& srdf_model);
644 LinkModel* constructLinkModel(
const urdf::Link* urdf_link);
647 shapes::ShapePtr constructShape(
const urdf::Geometry* geom);
#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.