Definition of a kinematic model. This class is not thread safe, however multiple instances can be created. More...
#include <kinematic_model.h>
Classes | |
class | AttachedBodyModel |
Class defining bodies that can be attached to robot links. This is useful when handling objects picked up by the robot. More... | |
class | FixedJointModel |
A fixed joint. More... | |
class | FloatingJointModel |
A floating joint. More... | |
struct | GroupConfig |
class | JointModel |
A joint from the robot. Contains the transform applied by the joint type. More... | |
class | JointModelGroup |
class | LinkModel |
A link from the robot. Contains the constant transform applied to the link and its geometry. More... | |
struct | MultiDofConfig |
class | PlanarJointModel |
A planar joint. More... | |
class | PrismaticJointModel |
A prismatic joint. More... | |
class | RevoluteJointModel |
A revolute joint. More... | |
Public Member Functions | |
void | addAttachedBodyModel (const std::string &link_name, AttachedBodyModel *att_body_model) |
bool | addModelGroup (const GroupConfig &group) |
void | clearAllAttachedBodyModels () |
void | clearLinkAttachedBodyModel (const std::string &link_name, const std::string &att_name) |
void | clearLinkAttachedBodyModels (const std::string &link_name) |
void | copyFrom (const KinematicModel &source) |
void | exclusiveLock (void) const |
Provide interface to get an exclusive lock to change the model. Use carefully! | |
void | exclusiveUnlock (void) const |
Provide interface to release an exclusive lock. Use carefully! | |
std::vector< const AttachedBodyModel * > | getAttachedBodyModels () const |
std::vector< std::string > | getChildJointModelNames (const LinkModel *parent) const |
Get the set of joint names that follow a parent link in the kinematic chain. | |
std::vector< std::string > | getChildJointModelNames (const JointModel *parent) const |
Get the set of joint names that follow a parent joint in the kinematic chain. | |
void | getChildJointModels (const LinkModel *parent, std::vector< const JointModel * > &links) const |
Get the set of joint models that follow a parent link in the kinematic chain. | |
void | getChildJointModels (const JointModel *parent, std::vector< const JointModel * > &links) const |
Get the set of joint models that follow a parent joint in the kinematic chain. | |
std::vector< std::string > | getChildLinkModelNames (const LinkModel *parent) const |
Get the set of link names that follow a parent link in the kinematic chain. | |
std::vector< std::string > | getChildLinkModelNames (const JointModel *parent) const |
Get the set of link names that follow a parent link in the kinematic chain. | |
void | getChildLinkModels (const LinkModel *parent, std::vector< const LinkModel * > &links) const |
Get the set of link models that follow a parent link in the kinematic chain. | |
void | getChildLinkModels (const JointModel *parent, std::vector< const LinkModel * > &links) const |
Get the set of link models that follow a parent joint in the kinematic chain. | |
const JointModel * | getJointModel (const std::string &joint) const |
Get a joint by its name. | |
const std::map< std::string, GroupConfig > & | getJointModelGroupConfigMap () const |
const std::map< std::string, JointModelGroup * > & | getJointModelGroupMap () const |
void | getJointModelNames (std::vector< std::string > &joints) const |
Get the array of joint names, in the order they appear in the robot state. | |
const std::vector< JointModel * > & | getJointModels () const |
Get the array of joints, in the order they appear in the robot state. | |
const LinkModel * | getLinkModel (const std::string &link) const |
Get a link by its name. | |
void | getLinkModelNames (std::vector< std::string > &links) const |
Get the link names in the order they should be updated. | |
const std::vector< LinkModel * > & | getLinkModels () const |
Get the array of joints, in the order they should be updated. | |
const std::vector< LinkModel * > & | getLinkModelsWithCollisionGeometry () const |
const JointModelGroup * | getModelGroup (const std::string &name) const |
void | getModelGroupNames (std::vector< std::string > &getModelGroupNames) const |
const std::string & | getName (void) const |
General the model name. | |
std::string | getRobotName () const |
const JointModel * | getRoot (void) const |
Get the root joint. | |
bool | hasJointModel (const std::string &name) const |
Check if a joint exists. | |
bool | hasLinkModel (const std::string &name) const |
Check if a link exists. | |
bool | hasModelGroup (const std::string &group) const |
KinematicModel (const KinematicModel &source) | |
Construct a kinematic model from another one. | |
KinematicModel (const urdf::Model &model, const std::vector< GroupConfig > &group_configs, const std::vector< MultiDofConfig > &multi_dof_configs) | |
Construct a kinematic model from a parsed description and a list of planning groups. | |
void | printModelInfo (std::ostream &out=std::cout) const |
Print information about the constructed model. | |
void | removeModelGroup (const std::string &group) |
void | replaceAttachedBodyModels (const std::string &link_name, std::vector< AttachedBodyModel * > &attached_body_vector) |
void | sharedLock (void) const |
Provide interface to get a shared lock for reading model data. | |
void | sharedUnlock (void) const |
Provide interface to release a shared lock. | |
~KinematicModel (void) | |
Destructor. Clear all memory. | |
Private Member Functions | |
void | buildGroups (const std::vector< GroupConfig > &) |
JointModel * | buildRecursive (LinkModel *parent, const urdf::Link *link, const std::vector< MultiDofConfig > &multi_dof_configs) |
JointModel * | constructJointModel (const urdf::Joint *urdfJointModel, const urdf::Link *child_link, const std::vector< MultiDofConfig > &multi_dof_configs) |
LinkModel * | constructLinkModel (const urdf::Link *urdfLink) |
shapes::Shape * | constructShape (const urdf::Geometry *geom) |
JointModel * | copyJointModel (const JointModel *joint) |
JointModel * | copyRecursive (LinkModel *parent, const LinkModel *link) |
Private Attributes | |
std::map< std::string, GroupConfig > | joint_model_group_config_map_ |
std::map< std::string, JointModelGroup * > | joint_model_group_map_ |
std::map< std::string, JointModel * > | joint_model_map_ |
A map from joint names to their instances. | |
std::vector< JointModel * > | joint_model_vector_ |
The vector of joints in the model, in the order they appear in the state vector. | |
std::map< std::string, LinkModel * > | link_model_map_ |
A map from link names to their instances. | |
std::vector< LinkModel * > | link_model_vector_ |
The vector of links that are updated when computeTransforms() is called, in the order they are updated. | |
std::vector< LinkModel * > | link_models_with_collision_geometry_vector_ |
Only links that have collision geometry specified. | |
boost::shared_mutex | lock_ |
Shared lock for changing models. | |
std::string | model_name_ |
The name of the model. | |
JointModel * | root_ |
The root joint. |
Definition of a kinematic model. This class is not thread safe, however multiple instances can be created.
Definition at line 58 of file kinematic_model.h.
planning_models::KinematicModel::KinematicModel | ( | const KinematicModel & | source | ) |
Construct a kinematic model from another one.
Definition at line 67 of file kinematic_model.cpp.
planning_models::KinematicModel::KinematicModel | ( | const urdf::Model & | model, |
const std::vector< GroupConfig > & | group_configs, | ||
const std::vector< MultiDofConfig > & | multi_dof_configs | ||
) |
Construct a kinematic model from a parsed description and a list of planning groups.
Definition at line 49 of file kinematic_model.cpp.
Destructor. Clear all memory.
Definition at line 72 of file kinematic_model.cpp.
void planning_models::KinematicModel::addAttachedBodyModel | ( | const std::string & | link_name, |
AttachedBodyModel * | att_body_model | ||
) |
Definition at line 774 of file kinematic_model.cpp.
bool planning_models::KinematicModel::addModelGroup | ( | const GroupConfig & | group | ) |
Definition at line 180 of file kinematic_model.cpp.
void planning_models::KinematicModel::buildGroups | ( | const std::vector< GroupConfig > & | group_configs | ) | [private] |
Definition at line 133 of file kinematic_model.cpp.
planning_models::KinematicModel::JointModel * planning_models::KinematicModel::buildRecursive | ( | LinkModel * | parent, |
const urdf::Link * | link, | ||
const std::vector< MultiDofConfig > & | multi_dof_configs | ||
) | [private] |
Definition at line 282 of file kinematic_model.cpp.
Definition at line 718 of file kinematic_model.cpp.
void planning_models::KinematicModel::clearLinkAttachedBodyModel | ( | const std::string & | link_name, |
const std::string & | att_name | ||
) |
Definition at line 762 of file kinematic_model.cpp.
void planning_models::KinematicModel::clearLinkAttachedBodyModels | ( | const std::string & | link_name | ) |
Definition at line 737 of file kinematic_model.cpp.
planning_models::KinematicModel::JointModel * planning_models::KinematicModel::constructJointModel | ( | const urdf::Joint * | urdfJointModel, |
const urdf::Link * | child_link, | ||
const std::vector< MultiDofConfig > & | multi_dof_configs | ||
) | [private] |
Definition at line 317 of file kinematic_model.cpp.
planning_models::KinematicModel::LinkModel * planning_models::KinematicModel::constructLinkModel | ( | const urdf::Link * | urdfLink | ) | [private] |
Definition at line 423 of file kinematic_model.cpp.
shapes::Shape * planning_models::KinematicModel::constructShape | ( | const urdf::Geometry * | geom | ) | [private] |
Definition at line 454 of file kinematic_model.cpp.
void planning_models::KinematicModel::copyFrom | ( | const KinematicModel & | source | ) |
Definition at line 108 of file kinematic_model.cpp.
planning_models::KinematicModel::JointModel * planning_models::KinematicModel::copyJointModel | ( | const JointModel * | joint | ) | [private] |
Definition at line 582 of file kinematic_model.cpp.
planning_models::KinematicModel::JointModel * planning_models::KinematicModel::copyRecursive | ( | LinkModel * | parent, |
const LinkModel * | link | ||
) | [private] |
Definition at line 561 of file kinematic_model.cpp.
void planning_models::KinematicModel::exclusiveLock | ( | void | ) | const |
Provide interface to get an exclusive lock to change the model. Use carefully!
Definition at line 84 of file kinematic_model.cpp.
void planning_models::KinematicModel::exclusiveUnlock | ( | void | ) | const |
Provide interface to release an exclusive lock. Use carefully!
Definition at line 89 of file kinematic_model.cpp.
std::vector< const planning_models::KinematicModel::AttachedBodyModel * > planning_models::KinematicModel::getAttachedBodyModels | ( | void | ) | const |
Definition at line 727 of file kinematic_model.cpp.
std::vector< std::string > planning_models::KinematicModel::getChildJointModelNames | ( | const LinkModel * | parent | ) | const |
Get the set of joint names that follow a parent link in the kinematic chain.
Definition at line 695 of file kinematic_model.cpp.
std::vector< std::string > planning_models::KinematicModel::getChildJointModelNames | ( | const JointModel * | parent | ) | const |
Get the set of joint names that follow a parent joint in the kinematic chain.
Definition at line 707 of file kinematic_model.cpp.
void planning_models::KinematicModel::getChildJointModels | ( | const LinkModel * | parent, |
std::vector< const JointModel * > & | links | ||
) | const |
Get the set of joint models that follow a parent link in the kinematic chain.
void planning_models::KinematicModel::getChildJointModels | ( | const JointModel * | parent, |
std::vector< const JointModel * > & | links | ||
) | const |
Get the set of joint models that follow a parent joint in the kinematic chain.
std::vector< std::string > planning_models::KinematicModel::getChildLinkModelNames | ( | const LinkModel * | parent | ) | const |
Get the set of link names that follow a parent link in the kinematic chain.
Definition at line 671 of file kinematic_model.cpp.
std::vector< std::string > planning_models::KinematicModel::getChildLinkModelNames | ( | const JointModel * | parent | ) | const |
Get the set of link names that follow a parent link in the kinematic chain.
Definition at line 683 of file kinematic_model.cpp.
void planning_models::KinematicModel::getChildLinkModels | ( | const LinkModel * | parent, |
std::vector< const LinkModel * > & | links | ||
) | const |
Get the set of link models that follow a parent link in the kinematic chain.
void planning_models::KinematicModel::getChildLinkModels | ( | const JointModel * | parent, |
std::vector< const LinkModel * > & | links | ||
) | const |
Get the set of link models that follow a parent joint in the kinematic chain.
const planning_models::KinematicModel::JointModel * planning_models::KinematicModel::getJointModel | ( | const std::string & | joint | ) | const |
Get a joint by its name.
Definition at line 512 of file kinematic_model.cpp.
const std::map<std::string, GroupConfig>& planning_models::KinematicModel::getJointModelGroupConfigMap | ( | ) | const [inline] |
Definition at line 687 of file kinematic_model.h.
const std::map<std::string, JointModelGroup*>& planning_models::KinematicModel::getJointModelGroupMap | ( | ) | const [inline] |
Definition at line 682 of file kinematic_model.h.
void planning_models::KinematicModel::getJointModelNames | ( | std::vector< std::string > & | joints | ) | const |
Get the array of joint names, in the order they appear in the robot state.
Definition at line 553 of file kinematic_model.cpp.
const std::vector<JointModel*>& planning_models::KinematicModel::getJointModels | ( | ) | const [inline] |
Get the array of joints, in the order they appear in the robot state.
Definition at line 632 of file kinematic_model.h.
const planning_models::KinematicModel::LinkModel * planning_models::KinematicModel::getLinkModel | ( | const std::string & | link | ) | const |
Get a link by its name.
Definition at line 524 of file kinematic_model.cpp.
void planning_models::KinematicModel::getLinkModelNames | ( | std::vector< std::string > & | links | ) | const |
Get the link names in the order they should be updated.
Definition at line 544 of file kinematic_model.cpp.
const std::vector<LinkModel*>& planning_models::KinematicModel::getLinkModels | ( | ) | const [inline] |
Get the array of joints, in the order they should be updated.
Definition at line 638 of file kinematic_model.h.
const std::vector<LinkModel*>& planning_models::KinematicModel::getLinkModelsWithCollisionGeometry | ( | ) | const [inline] |
Definition at line 643 of file kinematic_model.h.
const JointModelGroup* planning_models::KinematicModel::getModelGroup | ( | const std::string & | name | ) | const [inline] |
Definition at line 676 of file kinematic_model.h.
void planning_models::KinematicModel::getModelGroupNames | ( | std::vector< std::string > & | getModelGroupNames | ) | const |
Definition at line 536 of file kinematic_model.cpp.
const std::string & planning_models::KinematicModel::getName | ( | void | ) | const |
General the model name.
Definition at line 104 of file kinematic_model.cpp.
std::string planning_models::KinematicModel::getRobotName | ( | ) | const [inline] |
Definition at line 705 of file kinematic_model.h.
const planning_models::KinematicModel::JointModel * planning_models::KinematicModel::getRoot | ( | void | ) | const |
Get the root joint.
Definition at line 492 of file kinematic_model.cpp.
bool planning_models::KinematicModel::hasJointModel | ( | const std::string & | name | ) | const |
Check if a joint exists.
Definition at line 497 of file kinematic_model.cpp.
bool planning_models::KinematicModel::hasLinkModel | ( | const std::string & | name | ) | const |
Check if a link exists.
Definition at line 502 of file kinematic_model.cpp.
bool planning_models::KinematicModel::hasModelGroup | ( | const std::string & | group | ) | const |
Definition at line 507 of file kinematic_model.cpp.
void planning_models::KinematicModel::printModelInfo | ( | std::ostream & | out = std::cout | ) | const |
Print information about the constructed model.
void planning_models::KinematicModel::removeModelGroup | ( | const std::string & | group | ) |
Definition at line 173 of file kinematic_model.cpp.
void planning_models::KinematicModel::replaceAttachedBodyModels | ( | const std::string & | link_name, |
std::vector< AttachedBodyModel * > & | attached_body_vector | ||
) |
Definition at line 748 of file kinematic_model.cpp.
void planning_models::KinematicModel::sharedLock | ( | void | ) | const |
Provide interface to get a shared lock for reading model data.
Definition at line 94 of file kinematic_model.cpp.
void planning_models::KinematicModel::sharedUnlock | ( | void | ) | const |
Provide interface to release a shared lock.
Definition at line 99 of file kinematic_model.cpp.
std::map<std::string, GroupConfig> planning_models::KinematicModel::joint_model_group_config_map_ [private] |
Definition at line 736 of file kinematic_model.h.
std::map<std::string, JointModelGroup*> planning_models::KinematicModel::joint_model_group_map_ [private] |
Definition at line 735 of file kinematic_model.h.
std::map<std::string, JointModel*> planning_models::KinematicModel::joint_model_map_ [private] |
A map from joint names to their instances.
Definition at line 721 of file kinematic_model.h.
std::vector<JointModel*> planning_models::KinematicModel::joint_model_vector_ [private] |
The vector of joints in the model, in the order they appear in the state vector.
Definition at line 724 of file kinematic_model.h.
std::map<std::string, LinkModel*> planning_models::KinematicModel::link_model_map_ [private] |
A map from link names to their instances.
Definition at line 718 of file kinematic_model.h.
std::vector<LinkModel*> planning_models::KinematicModel::link_model_vector_ [private] |
The vector of links that are updated when computeTransforms() is called, in the order they are updated.
Definition at line 727 of file kinematic_model.h.
std::vector<LinkModel*> planning_models::KinematicModel::link_models_with_collision_geometry_vector_ [private] |
Only links that have collision geometry specified.
Definition at line 730 of file kinematic_model.h.
boost::shared_mutex planning_models::KinematicModel::lock_ [mutable, private] |
Shared lock for changing models.
Definition at line 712 of file kinematic_model.h.
std::string planning_models::KinematicModel::model_name_ [private] |
The name of the model.
Definition at line 715 of file kinematic_model.h.
JointModel* planning_models::KinematicModel::root_ [private] |
The root joint.
Definition at line 733 of file kinematic_model.h.