Public Member Functions | Private Member Functions | Static Private Member Functions | Private Attributes | Friends
robot_state::RobotState Class Reference

Definition of a kinematic state - the parts of the robot state which can change. Const members are thread safe. More...

#include <robot_state.h>

List of all members.

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 AttachedBodygetAttachedBody (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.
JointStategetJointState (const std::string &joint) const
 Get a joint state by its name.
JointStategetJointState (const robot_model::JointModel *jmodel) const
const JointStateGroupgetJointStateGroup (const std::string &name) const
 Get a group by its name.
JointStateGroupgetJointStateGroup (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.
LinkStategetLinkState (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::RandomNumberGeneratorgetRandomNumberGenerator ()
 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.
RobotStateoperator= (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

Detailed Description

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.


Constructor & Destructor Documentation

Create a state corresponding to a given kinematic model.

Definition at line 41 of file robot_state.cpp.

Copy constructor.

Definition at line 100 of file robot_state.cpp.

Definition at line 139 of file robot_state.cpp.


Member Function Documentation

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.

Parameters:
idThe string id associated with the attached body
shapesThe shapes that make up the attached body
attach_transThe desired transform between this link and the attached body
touch_linksThe set of links that the attached body is allowed to touch
link_nameThe 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.

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.

Definition at line 156 of file robot_state.h.

Get a group by its name.

Definition at line 345 of file robot_state.cpp.

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.

Parameters:
arrThe returned marker array
link_namesThe list of link names for which the markers should be created.
colorThe color for the marker
nsThe namespace for the markers
durThe 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.

Parameters:
arrThe returned marker array
link_namesThe list of link names for which the markers should be created.

Definition at line 658 of file robot_state.cpp.

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.

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.

Check if all joints satisfy their bounds.

Definition at line 330 of file robot_state.cpp.

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.


Friends And Related Function Documentation

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.


Member Data Documentation

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.

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.

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.

The states for all the links in the robot.

Definition at line 307 of file robot_state.h.

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.


The documentation for this class was generated from the following files:


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:48