Definition of a kinematic state - the parts of the robot state which can change. Const members are thread safe. More...
#include <robot_state.h>
Public Member Functions | |
void | attachBody (AttachedBody *attached_body) |
Attach a body to this state. | |
void | attachBody (const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &attach_trans, const std::set< std::string > &touch_links, const std::string &link_name, const sensor_msgs::JointState &detach_posture=sensor_msgs::JointState()) |
Attach a body to a link. | |
void | attachBody (const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &attach_trans, const std::vector< std::string > &touch_links, const std::string &link_name, const sensor_msgs::JointState &detach_posture=sensor_msgs::JointState()) |
void | clearAttachedBodies (const std::string &link_name) |
Clear the bodies attached to a specific link. | |
void | clearAttachedBodies () |
Clear all attached bodies. | |
bool | clearAttachedBody (const std::string &id) |
Remove the attached body named id. Return false if the object was not found (and thus not removed). Return true on success. | |
void | computeAABB (std::vector< double > &aabb) const |
Compute the axis-aligned bounding box for this particular robot state. aabb will have 6 values: xmin, xmax, ymin, ymax, zmin, zmax. | |
double | distance (const RobotState &state) const |
Get the distance between this state and another one. This distance does not consider topology -- it is only the L2 norm on the joint vector. | |
void | enforceBounds () |
Make sure all state variables are within bounds and normalized. | |
void | getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies) const |
Get all bodies attached to the model corresponding to this state. | |
const AttachedBody * | getAttachedBody (const std::string &name) const |
Get the attached body named name. Return NULL if not found. | |
const Eigen::Affine3d & | getFrameTransform (const std::string &id) const |
Get the transform corresponding to the frame id. This will be known if id is a link name or an attached body id. Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be successful or not. | |
JointState * | getJointState (const std::string &joint) const |
Get a joint state by its name. | |
JointState * | getJointState (const robot_model::JointModel *jmodel) const |
const JointStateGroup * | getJointStateGroup (const std::string &name) const |
Get a group by its name. | |
JointStateGroup * | getJointStateGroup (const std::string &name) |
Get a group by its name. | |
const std::map< std::string, JointStateGroup * > & | getJointStateGroupMap () const |
Get a map that returns JointStateGroups corresponding to names. | |
void | getJointStateGroupNames (std::vector< std::string > &names) const |
Get the names of all joint groups in the model corresponding to this state. | |
const std::vector< JointState * > & | getJointStateVector () const |
Get a vector of joint state corresponding to this kinematic state. | |
LinkState * | getLinkState (const std::string &link) const |
Get a link state by its name. | |
const std::vector< LinkState * > & | getLinkStateVector () const |
Get all the maintained link states, in the same order as the link models maintained by the kinematic model. | |
random_numbers::RandomNumberGenerator & | getRandomNumberGenerator () |
Return the instance of a random number generator. | |
void | getRobotMarkers (visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false) const |
Get a MarkerArray that fully describes the robot markers for a given robot. | |
void | getRobotMarkers (visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, bool include_attached=false) const |
Get a MarkerArray that fully describes the robot markers for a given robot. | |
const robot_model::RobotModelConstPtr & | getRobotModel () const |
Get the kinematic model corresponding to this state. | |
const Eigen::Affine3d & | getRootTransform () const |
Get the global transform applied to the entire tree of links. | |
std::string | getStateTreeString (const std::string &prefix="") const |
Returns a string showing the joint and link state tree with transforms. | |
void | getStateValues (std::vector< double > &joint_state_values) const |
Get the joint state values. The order in which the values are specified matches the order of the joints in the RobotModel corresponding to this state. | |
void | getStateValues (std::map< std::string, double > &joint_state_values) const |
Get the joint state values as a map between joint state names and values. | |
void | getStateValues (sensor_msgs::JointState &msg) const |
Get the joint state values in a sensor_msgs::JointState msg. | |
unsigned int | getVariableCount () const |
Get the number of (active) DOFs in the model corresponding to this state. | |
bool | hasAttachedBody (const std::string &id) const |
Check if an attached body named id exists in this state. | |
bool | hasJointState (const std::string &joint) const |
Check if a joint is part of this state. | |
bool | hasJointStateGroup (const std::string &name) const |
Check if a group exists. | |
bool | hasLinkState (const std::string &link) const |
Check if a link is updated by this state. | |
double | infinityNormDistance (const robot_state::RobotState *other) const |
Get the infinity norm distance between two kinematic states. | |
void | interpolate (const RobotState &to, const double t, RobotState &dest) const |
Interpolate between two states. | |
bool | knowsFrameTransform (const std::string &id) const |
Check if a transform to the frame id is known. This will be known if id is a link name or an attached body id. | |
RobotState & | operator= (const RobotState &other) |
Assignment operator. Copies everything, including attached bodies (clones them) | |
void | printStateInfo (std::ostream &out=std::cout) const |
Print information about the constructed model. | |
void | printTransforms (std::ostream &out=std::cout) const |
Print the pose of every link. | |
RobotState (const robot_model::RobotModelConstPtr &kinematic_model) | |
Create a state corresponding to a given kinematic model. | |
RobotState (const RobotState &state) | |
Copy constructor. | |
bool | satisfiesBounds (const std::vector< std::string > &joints) const |
Check if a particular set of joints satisifes its bounds. | |
bool | satisfiesBounds (const std::string &joint) const |
Check if a joint satisfies its bounds. | |
bool | satisfiesBounds () const |
Check if all joints satisfy their bounds. | |
void | setAttachedBodyUpdateCallback (const AttachedBodyCallback &callback) |
void | setRootTransform (const Eigen::Affine3d &transform) |
Set the global transform applied to the entire tree of links. | |
bool | setStateValues (const std::vector< double > &joint_state_values) |
Set the joint state values from a vector of values. Assumes that the order of the values matches the order of the joints in the state. Should only be used for fast setting of joint values. | |
void | setStateValues (const std::map< std::string, double > &joint_state_map) |
Set the joint state values from a map of values (matching string ids to actual joint values) | |
void | setStateValues (const std::map< std::string, double > &joint_state_map, std::vector< std::string > &missing) |
Set the joint state values from a map of values (matching string ids to actual joint values). Also returns the set of joint names for which joint states have not been provided. | |
void | setStateValues (const sensor_msgs::JointState &msg) |
Set the joint state values from a joint state message. | |
void | setStateValues (const std::vector< std::string > &joint_names, const std::vector< double > &joint_values) |
Set the joint state values for an array of variable names, given the values are specified in the same order as the names. This is just a convenience call equivalent to passing a map from string to double. | |
void | setToDefaultValues () |
Set all joints to their default values. | |
void | setToRandomValues () |
Sample a random state in accordance with the type of joints employed. | |
void | updateLinkTransforms () |
Perform forward kinematics with the current values and update the link transforms. | |
bool | updateStateWithLinkAt (const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false) |
Update the state after setting a particular link to the input global transform pose. | |
~RobotState () | |
Private Member Functions | |
void | buildState () |
void | copyFrom (const RobotState &ks) |
void | getStateTreeJointString (std::stringstream &ss, const robot_state::JointState *js, const std::string &prefix, bool last) const |
void | printTransform (const std::string &st, const Eigen::Affine3d &t, std::ostream &out=std::cout) const |
Static Private Member Functions | |
static void | getPoseString (std::stringstream &ss, const Eigen::Affine3d &mtx, const std::string &pfx="") |
Private Attributes | |
std::map< std::string, AttachedBody * > | attached_body_map_ |
The attached bodies that are part of this state (from all links) | |
AttachedBodyCallback | attached_body_update_callback_ |
This event is called when there is a change in the attached bodies for this state; The event specifies the body that changed and whether it was just attached or about to be detached. | |
std::map< std::string, JointStateGroup * > | joint_state_group_map_ |
A map from group names to instances of the group state. | |
std::map< std::string, JointState * > | joint_state_map_ |
std::vector< JointState * > | joint_state_vector_ |
robot_model::RobotModelConstPtr | kinematic_model_ |
std::map< std::string, LinkState * > | link_state_map_ |
A map from link names to their corresponding states. | |
std::vector< LinkState * > | link_state_vector_ |
The states for all the links in the robot. | |
boost::scoped_ptr < random_numbers::RandomNumberGenerator > | rng_ |
For certain operations a state needs a random number generator. However, it may be slightly expensive to allocate the random number generator if many state instances are generated. For this reason, the generator is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but call getRandomNumberGenerator() instead. | |
Eigen::Affine3d | root_transform_ |
Additional transform to be applied to the tree of links. | |
Friends | |
class | JointState |
class | LinkState |
Definition of a kinematic state - the parts of the robot state which can change. Const members are thread safe.
Definition at line 54 of file robot_state.h.
robot_state::RobotState::RobotState | ( | const robot_model::RobotModelConstPtr & | kinematic_model | ) |
Create a state corresponding to a given kinematic model.
Definition at line 41 of file robot_state.cpp.
robot_state::RobotState::RobotState | ( | const RobotState & | state | ) |
Copy constructor.
Definition at line 100 of file robot_state.cpp.
Definition at line 139 of file robot_state.cpp.
void robot_state::RobotState::attachBody | ( | AttachedBody * | attached_body | ) |
Attach a body to this state.
Definition at line 422 of file robot_state.cpp.
void robot_state::RobotState::attachBody | ( | const std::string & | id, |
const std::vector< shapes::ShapeConstPtr > & | shapes, | ||
const EigenSTL::vector_Affine3d & | attach_trans, | ||
const std::set< std::string > & | touch_links, | ||
const std::string & | link_name, | ||
const sensor_msgs::JointState & | detach_posture = sensor_msgs::JointState() |
||
) |
Attach a body to a link.
id | The string id associated with the attached body |
shapes | The shapes that make up the attached body |
attach_trans | The desired transform between this link and the attached body |
touch_links | The set of links that the attached body is allowed to touch |
link_name | The link to attach to |
Definition at line 435 of file robot_state.cpp.
void robot_state::RobotState::attachBody | ( | const std::string & | id, |
const std::vector< shapes::ShapeConstPtr > & | shapes, | ||
const EigenSTL::vector_Affine3d & | attach_trans, | ||
const std::vector< std::string > & | touch_links, | ||
const std::string & | link_name, | ||
const sensor_msgs::JointState & | detach_posture = sensor_msgs::JointState() |
||
) |
Definition at line 454 of file robot_state.cpp.
void robot_state::RobotState::buildState | ( | ) | [private] |
Definition at line 55 of file robot_state.cpp.
void robot_state::RobotState::clearAttachedBodies | ( | const std::string & | link_name | ) |
Clear the bodies attached to a specific link.
Definition at line 487 of file robot_state.cpp.
Clear all attached bodies.
Definition at line 473 of file robot_state.cpp.
bool robot_state::RobotState::clearAttachedBody | ( | const std::string & | id | ) |
Remove the attached body named id. Return false if the object was not found (and thus not removed). Return true on success.
Definition at line 503 of file robot_state.cpp.
void robot_state::RobotState::computeAABB | ( | std::vector< double > & | aabb | ) | const |
Compute the axis-aligned bounding box for this particular robot state. aabb will have 6 values: xmin, xmax, ymin, ymax, zmin, zmax.
Definition at line 560 of file robot_state.cpp.
void robot_state::RobotState::copyFrom | ( | const RobotState & | ks | ) | [private] |
Definition at line 111 of file robot_state.cpp.
double robot_state::RobotState::distance | ( | const RobotState & | state | ) | const |
Get the distance between this state and another one. This distance does not consider topology -- it is only the L2 norm on the joint vector.
Definition at line 583 of file robot_state.cpp.
Make sure all state variables are within bounds and normalized.
Definition at line 338 of file robot_state.cpp.
void robot_state::RobotState::getAttachedBodies | ( | std::vector< const AttachedBody * > & | attached_bodies | ) | const |
Get all bodies attached to the model corresponding to this state.
Definition at line 465 of file robot_state.cpp.
const robot_state::AttachedBody * robot_state::RobotState::getAttachedBody | ( | const std::string & | name | ) | const |
Get the attached body named name. Return NULL if not found.
Definition at line 410 of file robot_state.cpp.
const Eigen::Affine3d & robot_state::RobotState::getFrameTransform | ( | const std::string & | id | ) | const |
Get the transform corresponding to the frame id. This will be known if id is a link name or an attached body id. Return identity when no transform is available. Use knowsFrameTransform() to test if this function will be successful or not.
Definition at line 599 of file robot_state.cpp.
robot_state::JointState * robot_state::RobotState::getJointState | ( | const std::string & | joint | ) | const |
Get a joint state by its name.
Definition at line 381 of file robot_state.cpp.
JointState* robot_state::RobotState::getJointState | ( | const robot_model::JointModel * | jmodel | ) | const [inline] |
Definition at line 156 of file robot_state.h.
const robot_state::JointStateGroup * robot_state::RobotState::getJointStateGroup | ( | const std::string & | name | ) | const |
Get a group by its name.
Definition at line 345 of file robot_state.cpp.
robot_state::JointStateGroup * robot_state::RobotState::getJointStateGroup | ( | const std::string & | name | ) |
Get a group by its name.
Definition at line 352 of file robot_state.cpp.
const std::map<std::string, JointStateGroup*>& robot_state::RobotState::getJointStateGroupMap | ( | ) | const [inline] |
Get a map that returns JointStateGroups corresponding to names.
Definition at line 178 of file robot_state.h.
void robot_state::RobotState::getJointStateGroupNames | ( | std::vector< std::string > & | names | ) | const |
Get the names of all joint groups in the model corresponding to this state.
Definition at line 364 of file robot_state.cpp.
const std::vector<JointState*>& robot_state::RobotState::getJointStateVector | ( | ) | const [inline] |
Get a vector of joint state corresponding to this kinematic state.
Definition at line 166 of file robot_state.h.
robot_state::LinkState * robot_state::RobotState::getLinkState | ( | const std::string & | link | ) | const |
Get a link state by its name.
Definition at line 393 of file robot_state.cpp.
const std::vector<LinkState*>& robot_state::RobotState::getLinkStateVector | ( | ) | const [inline] |
Get all the maintained link states, in the same order as the link models maintained by the kinematic model.
Definition at line 172 of file robot_state.h.
void robot_state::RobotState::getPoseString | ( | std::stringstream & | ss, |
const Eigen::Affine3d & | mtx, | ||
const std::string & | pfx = "" |
||
) | [static, private] |
Definition at line 729 of file robot_state.cpp.
Return the instance of a random number generator.
Definition at line 48 of file robot_state.cpp.
void robot_state::RobotState::getRobotMarkers | ( | visualization_msgs::MarkerArray & | arr, |
const std::vector< std::string > & | link_names, | ||
const std_msgs::ColorRGBA & | color, | ||
const std::string & | ns, | ||
const ros::Duration & | dur, | ||
bool | include_attached = false |
||
) | const |
Get a MarkerArray that fully describes the robot markers for a given robot.
arr | The returned marker array |
link_names | The list of link names for which the markers should be created. |
color | The color for the marker |
ns | The namespace for the markers |
dur | The ros::Duration for which the markers should stay visible |
Definition at line 639 of file robot_state.cpp.
void robot_state::RobotState::getRobotMarkers | ( | visualization_msgs::MarkerArray & | arr, |
const std::vector< std::string > & | link_names, | ||
bool | include_attached = false |
||
) | const |
Get a MarkerArray that fully describes the robot markers for a given robot.
arr | The returned marker array |
link_names | The list of link names for which the markers should be created. |
Definition at line 658 of file robot_state.cpp.
const robot_model::RobotModelConstPtr& robot_state::RobotState::getRobotModel | ( | ) | const [inline] |
Get the kinematic model corresponding to this state.
Definition at line 109 of file robot_state.h.
const Eigen::Affine3d & robot_state::RobotState::getRootTransform | ( | ) | const |
Get the global transform applied to the entire tree of links.
Definition at line 269 of file robot_state.cpp.
void robot_state::RobotState::getStateTreeJointString | ( | std::stringstream & | ss, |
const robot_state::JointState * | js, | ||
const std::string & | prefix, | ||
bool | last | ||
) | const [private] |
Definition at line 743 of file robot_state.cpp.
std::string robot_state::RobotState::getStateTreeString | ( | const std::string & | prefix = "" | ) | const |
Returns a string showing the joint and link state tree with transforms.
Definition at line 773 of file robot_state.cpp.
void robot_state::RobotState::getStateValues | ( | std::vector< double > & | joint_state_values | ) | const |
Get the joint state values. The order in which the values are specified matches the order of the joints in the RobotModel corresponding to this state.
Definition at line 212 of file robot_state.cpp.
void robot_state::RobotState::getStateValues | ( | std::map< std::string, double > & | joint_state_values | ) | const |
Get the joint state values as a map between joint state names and values.
Definition at line 223 of file robot_state.cpp.
void robot_state::RobotState::getStateValues | ( | sensor_msgs::JointState & | msg | ) | const |
Get the joint state values in a sensor_msgs::JointState msg.
Definition at line 235 of file robot_state.cpp.
unsigned int robot_state::RobotState::getVariableCount | ( | ) | const [inline] |
Get the number of (active) DOFs in the model corresponding to this state.
Definition at line 115 of file robot_state.h.
bool robot_state::RobotState::hasAttachedBody | ( | const std::string & | id | ) | const |
Check if an attached body named id exists in this state.
Definition at line 405 of file robot_state.cpp.
bool robot_state::RobotState::hasJointState | ( | const std::string & | joint | ) | const |
Check if a joint is part of this state.
Definition at line 371 of file robot_state.cpp.
bool robot_state::RobotState::hasJointStateGroup | ( | const std::string & | name | ) | const |
Check if a group exists.
Definition at line 359 of file robot_state.cpp.
bool robot_state::RobotState::hasLinkState | ( | const std::string & | link | ) | const |
Check if a link is updated by this state.
Definition at line 376 of file robot_state.cpp.
double robot_state::RobotState::infinityNormDistance | ( | const robot_state::RobotState * | other | ) | const |
Get the infinity norm distance between two kinematic states.
Definition at line 294 of file robot_state.cpp.
void robot_state::RobotState::interpolate | ( | const RobotState & | to, |
const double | t, | ||
RobotState & | dest | ||
) | const |
Interpolate between two states.
Definition at line 592 of file robot_state.cpp.
bool robot_state::RobotState::knowsFrameTransform | ( | const std::string & | id | ) | const |
Check if a transform to the frame id is known. This will be known if id is a link name or an attached body id.
Definition at line 627 of file robot_state.cpp.
robot_state::RobotState & robot_state::RobotState::operator= | ( | const RobotState & | other | ) |
Assignment operator. Copies everything, including attached bodies (clones them)
Definition at line 105 of file robot_state.cpp.
void robot_state::RobotState::printStateInfo | ( | std::ostream & | out = std::cout | ) | const |
Print information about the constructed model.
Definition at line 721 of file robot_state.cpp.
void robot_state::RobotState::printTransform | ( | const std::string & | st, |
const Eigen::Affine3d & | t, | ||
std::ostream & | out = std::cout |
||
) | const [private] |
Definition at line 782 of file robot_state.cpp.
void robot_state::RobotState::printTransforms | ( | std::ostream & | out = std::cout | ) | const |
Print the pose of every link.
Definition at line 791 of file robot_state.cpp.
bool robot_state::RobotState::satisfiesBounds | ( | const std::vector< std::string > & | joints | ) | const |
Check if a particular set of joints satisifes its bounds.
Definition at line 314 of file robot_state.cpp.
bool robot_state::RobotState::satisfiesBounds | ( | const std::string & | joint | ) | const |
Check if a joint satisfies its bounds.
Definition at line 308 of file robot_state.cpp.
bool robot_state::RobotState::satisfiesBounds | ( | ) | const |
Check if all joints satisfy their bounds.
Definition at line 330 of file robot_state.cpp.
void robot_state::RobotState::setAttachedBodyUpdateCallback | ( | const AttachedBodyCallback & | callback | ) |
Definition at line 714 of file robot_state.cpp.
void robot_state::RobotState::setRootTransform | ( | const Eigen::Affine3d & | transform | ) |
Set the global transform applied to the entire tree of links.
Definition at line 274 of file robot_state.cpp.
bool robot_state::RobotState::setStateValues | ( | const std::vector< double > & | joint_state_values | ) |
Set the joint state values from a vector of values. Assumes that the order of the values matches the order of the joints in the state. Should only be used for fast setting of joint values.
Definition at line 151 of file robot_state.cpp.
void robot_state::RobotState::setStateValues | ( | const std::map< std::string, double > & | joint_state_map | ) |
Set the joint state values from a map of values (matching string ids to actual joint values)
Definition at line 174 of file robot_state.cpp.
void robot_state::RobotState::setStateValues | ( | const std::map< std::string, double > & | joint_state_map, |
std::vector< std::string > & | missing | ||
) |
Set the joint state values from a map of values (matching string ids to actual joint values). Also returns the set of joint names for which joint states have not been provided.
Definition at line 182 of file robot_state.cpp.
void robot_state::RobotState::setStateValues | ( | const sensor_msgs::JointState & | msg | ) |
Set the joint state values from a joint state message.
Definition at line 191 of file robot_state.cpp.
void robot_state::RobotState::setStateValues | ( | const std::vector< std::string > & | joint_names, |
const std::vector< double > & | joint_values | ||
) |
Set the joint state values for an array of variable names, given the values are specified in the same order as the names. This is just a convenience call equivalent to passing a map from string to double.
Definition at line 201 of file robot_state.cpp.
Set all joints to their default values.
Definition at line 279 of file robot_state.cpp.
Sample a random state in accordance with the type of joints employed.
Definition at line 286 of file robot_state.cpp.
Perform forward kinematics with the current values and update the link transforms.
Definition at line 250 of file robot_state.cpp.
bool robot_state::RobotState::updateStateWithLinkAt | ( | const std::string & | link_name, |
const Eigen::Affine3d & | transform, | ||
bool | backward = false |
||
) |
Update the state after setting a particular link to the input global transform pose.
Definition at line 256 of file robot_state.cpp.
friend class JointState [friend] |
Definition at line 57 of file robot_state.h.
friend class LinkState [friend] |
Definition at line 56 of file robot_state.h.
std::map<std::string, AttachedBody*> robot_state::RobotState::attached_body_map_ [private] |
The attached bodies that are part of this state (from all links)
Definition at line 319 of file robot_state.h.
This event is called when there is a change in the attached bodies for this state; The event specifies the body that changed and whether it was just attached or about to be detached.
Definition at line 329 of file robot_state.h.
std::map<std::string, JointStateGroup*> robot_state::RobotState::joint_state_group_map_ [private] |
A map from group names to instances of the group state.
Definition at line 316 of file robot_state.h.
std::map<std::string, JointState*> robot_state::RobotState::joint_state_map_ [private] |
Definition at line 304 of file robot_state.h.
std::vector<JointState*> robot_state::RobotState::joint_state_vector_ [private] |
Definition at line 303 of file robot_state.h.
Definition at line 301 of file robot_state.h.
std::map<std::string, LinkState*> robot_state::RobotState::link_state_map_ [private] |
A map from link names to their corresponding states.
Definition at line 310 of file robot_state.h.
std::vector<LinkState*> robot_state::RobotState::link_state_vector_ [private] |
The states for all the links in the robot.
Definition at line 307 of file robot_state.h.
boost::scoped_ptr<random_numbers::RandomNumberGenerator> robot_state::RobotState::rng_ [private] |
For certain operations a state needs a random number generator. However, it may be slightly expensive to allocate the random number generator if many state instances are generated. For this reason, the generator is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but call getRandomNumberGenerator() instead.
Definition at line 325 of file robot_state.h.
Eigen::Affine3d robot_state::RobotState::root_transform_ [private] |
Additional transform to be applied to the tree of links.
Definition at line 313 of file robot_state.h.