38 #ifndef MOVEIT_CORE_ROBOT_MODEL_ 39 #define MOVEIT_CORE_ROBOT_MODEL_ 54 #include <Eigen/Geometry> 98 const urdf::ModelInterfaceSharedPtr&
getURDF()
const 318 std::map<std::string, double>& values)
const;
333 double margin = 0.0)
const;
340 double distance(
const double* state1,
const double* state2)
const;
341 void interpolate(
const double* from,
const double* to,
double t,
double* state)
const;
419 std::vector<std::string>& missing_variables)
const;
459 urdf::ModelInterfaceSharedPtr
urdf_;
584 void buildMimic(
const urdf::ModelInterface& urdf_model);
std::vector< const JointModel * > mimic_joints_
The set of mimic joints this model contains.
std::size_t getLinkGeometryCount() const
const std::vector< std::string > & getLinkModelNamesWithCollisionGeometry() const
Get the names of the link models that have some collision geometry associated to themselves.
const std::string & getName() const
The name of this link.
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.
const urdf::ModelInterfaceSharedPtr & getURDF() const
Get the parsed URDF model.
void printModelInfo(std::ostream &out) const
Print information about the constructed model.
const std::string & getName() const
Get the name of the joint.
Core components of MoveIt!
std::vector< LinkModel * > link_model_vector_
The vector of links that are updated when computeTransforms() is called, in the order they are update...
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...
std::vector< const JointModel * > joints_of_variable_
The joints that correspond to each variable index.
std::vector< std::string > link_model_names_vector_
The vector of link names that corresponds to link_model_vector_.
const std::string & getName() const
Get the model name.
const std::vector< const LinkModel * > & getLinkModelsWithCollisionGeometry() const
Get the link models that have some collision geometry associated to themselves.
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...
JointBoundsVector active_joint_models_bounds_
The bounds for all the active joint models.
static const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const LinkModel *link)
Get the latest link upwards the kinematic tree, which is only connected via fixed joints...
LinkModelMap link_model_map_
A map from link names to their instances.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
Compute the random values for a RobotState.
const JointModel * getJointOfVariable(int variable_index) const
void getVariableDefaultPositions(std::vector< double > &values) const
Compute the default values for a RobotState.
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.
const std::vector< JointModel * > & getActiveJointModels()
Get the array of joints that are active (not fixed, not mimic) in this model.
void buildJointInfo()
Compute helpful information about joints.
const LinkModel * getLinkModel(const std::string &link) const
Get a link by its name. Output error and return NULL when the link is missing.
const LinkModel * root_link_
The first physical link for the robot.
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a specific variable. Throw an exception of variable is not found.
std::size_t getVariableCount() const
Get the number of variables that describe this model.
JointModelGroupMap end_effectors_map_
The known end effectors.
std::map< std::string, JointModelGroup * > JointModelGroupMap
Map of names to instances for JointModelGroup.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the array of joints that are active (not fixed, not mimic) in this model.
~RobotModel()
Destructor. Clear all memory.
const JointModel * getRootJoint() const
Get the root joint. There will be one root joint unless the model is empty. This is either extracted ...
std::map< const LinkModel *, Eigen::Affine3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Affine3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
double distance(const double *state1, const double *state2) const
std::shared_ptr< Shape > ShapePtr
const std::vector< LinkModel * > & getLinkModels()
Get the array of links.
const JointModel * getJointOfVariable(const std::string &variable) const
const JointModelGroup * getEndEffector(const std::string &name) const
Get the joint group that corresponds to a given end-effector name.
const std::vector< const JointModelGroup * > & getEndEffectors() const
Get the map between end effector names and the groups they correspond to.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
void buildGroups(const srdf::Model &srdf_model)
Given a SRDF model describing the groups, build up the groups in this kinematic model.
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
void setKinematicsAllocators(const std::map< std::string, SolverAllocatorFn > &allocators)
A map of known kinematics solvers (associated to their group name)
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.
const std::vector< std::string > & getJointModelGroupNames() const
Get the names of all groups that are defined for this model.
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Output error and return NULL when the joint is missing.
std::string model_name_
The name of the model.
const std::vector< std::string > & getLinkModelNames() const
Get the link names (of all links)
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...
VariableIndexMap joint_variables_index_map_
The state includes all the joint variables that make up the joints the state consists of...
void computeDescendants()
For every joint, pre-compute the list of descendant joints & links.
int getJointIndex() const
Get the index of this joint within the robot model.
MOVEIT_CLASS_FORWARD(RobotModel)
JointModelGroupMap joint_model_group_map_
A map from group names to joint groups.
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...
void computeFixedTransforms(const LinkModel *link, const Eigen::Affine3d &transform, LinkTransformMap &associated_transforms)
Get the transforms between link and all its rigidly attached descendants.
std::size_t getJointModelCount() const
std::map< std::string, JointModel * > JointModelMap
Map of names to instances for JointModel.
const std::vector< const JointModel * > & getMimicJointModels() const
Get the array of mimic joints, in the order they appear in the robot state.
std::vector< const JointModel::Bounds * > JointBoundsVector
std::size_t variable_count_
Get the number of variables necessary to describe this model.
const std::vector< const JointModel * > & getSingleDOFJointModels() const
This is a list of all single-dof joints (including mimic joints)
const JointModel * computeCommonRoot(const JointModel *a, const JointModel *b) const
Given two joints, find their common root.
const JointModel * root_joint_
The root joint.
std::vector< const JointModel * > joint_model_vector_const_
The vector of joints in the model, in the order they appear in the state vector.
srdf::ModelConstSharedPtr srdf_
std::map< std::string, int > VariableIndexMap
Data type for holding mappings from variable names to their position in a state vector.
std::vector< const JointModelGroup * > joint_model_groups_const_
The array of joint model groups, in alphabetical order.
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
std::size_t getLinkModelCount() const
bool enforcePositionBounds(double *state) const
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints, in the order they appear in the robot state.
void buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
std::vector< const JointModel * > continuous_joint_model_vector_
The set of continuous joints this model contains.
std::vector< const JointModel * > single_dof_joints_
std::vector< const JointModelGroup * > end_effectors_
The array of end-effectors, in alphabetical order.
void updateMimicJoints(double *values) const
Update the variable values for the state of a group with respect to the mimic joints.
std::map< std::string, LinkModel * > LinkModelMap
Map of names to instances for LinkModel.
void computeCommonRoots()
For every pair of joints, pre-compute the common roots of the joints.
std::vector< JointModel * > active_joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
const LinkModel * getRootLink() const
Get the physical root link of the robot.
std::size_t link_geometry_count_
Total number of geometric shapes in this model.
JointModelMap joint_model_map_
A map from joint names to their instances.
std::vector< std::string > joint_model_names_vector_
The vector of joint names that corresponds to joint_model_vector_.
urdf::ModelInterfaceSharedPtr urdf_
bool hasJointModel(const std::string &name) const
Check if a joint exists. Return true if it does.
const std::string & getRootJointName() const
Return the name of the root joint. Throws an exception if there is no root joint. ...
std::string model_frame_
The reference frame for this model.
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector< double > &values) const
Compute the random values for a RobotState.
void buildGroupsInfo_Subgroups(const srdf::Model &srdf_model)
Compute helpful information about groups (that can be queried later)
const std::string & getRootLinkName() const
Get the name of the root link of the robot.
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...
bool isEmpty() const
Return true if the model is empty (has no root link, no joints)
std::vector< JointModel * > joint_model_vector_
The vector of joints in the model, in the order they appear in the state vector.
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...
void getMissingVariableNames(const std::vector< std::string > &variables, std::vector< std::string > &missing_variables) const
bool addJointModelGroup(const srdf::Model::Group &group)
Construct a JointModelGroup given a SRDF description group.
void interpolate(const double *from, const double *to, double t, double *state) const
A link from the robot. Contains the constant transform applied to the link and its geometry...
std::vector< int > active_joint_model_start_index_
int getVariableIndex(const std::string &variable) const
Get the index of a variable in the robot state.
bool hasEndEffector(const std::string &eef) const
Check if an end effector exists.
std::vector< const JointModel * > multi_dof_joints_
bool hasLinkModel(const std::string &name) const
Check if a link exists. Return true if it does.
void buildMimic(const urdf::ModelInterface &urdf_model)
Given the URDF model, build up the mimic joints (mutually constrained joints)
std::vector< JointModelGroup * > joint_model_groups_
The array of joint model groups, in alphabetical order.
const std::vector< const JointModel * > & getJointModels() const
Get the array of joints, in the order they appear in the robot state.
const std::vector< std::string > & getJointModelNames() const
Get the array of joint names, in the order they appear in the robot state.
Main namespace for MoveIt!
std::vector< std::string > joint_model_group_names_
A vector of all group names, in alphabetical order.
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.
std::vector< const LinkModel * > link_models_with_collision_geometry_vector_
Only links that have collision geometry specified.
std::vector< int > common_joint_roots_
For every two joints, the index of the common root for thw joints is stored.
const VariableBounds & getVariableBounds(const std::string &variable) const
Get the bounds for a variable. Throw an exception if the variable was not found.
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 JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
bool satisfiesPositionBounds(const double *state, double margin=0.0) const
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...
double getMaximumExtent() const
const std::vector< JointModelGroup * > & getJointModelGroups()
Get the available joint groups.
const std::vector< const LinkModel * > & getLinkModels() const
Get the array of links.
bool hasJointModelGroup(const std::string &group) const
Check if the JointModelGroup group exists.
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_.
const srdf::ModelConstSharedPtr & getSRDF() const
Get the parsed SRDF model.
const std::vector< const JointModel * > & getMultiDOFJointModels() const
This is a list of all multi-dof joints.
void getVariableDefaultPositions(double *values) const
Compute the default values for a RobotState.
const std::vector< const JointModelGroup * > & getJointModelGroups() const
Get the available joint groups.