Representation of a robot's state. This includes position, velocity, acceleration and effort. More...

#include <robot_state.h>

Public Member Functions
void	computeAABB (std::vector< double > &aabb) const
	Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz)
void	computeAABB (std::vector< double > &aabb)
	Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz)
const Eigen::Affine3d &	getFrameTransform (const std::string &id)
	Get the transformation matrix from the model frame to the frame identified by id.
const Eigen::Affine3d &	getFrameTransform (const std::string &id) const
	Get the transformation matrix from the model frame to the frame identified by id.
const JointModel *	getJointModel (const std::string &joint) const
	Get the model of a particular joint.
const JointModelGroup *	getJointModelGroup (const std::string &group) const
	Get the model of a particular joint group.
const LinkModel *	getLinkModel (const std::string &link) const
	Get the model of a particular link.
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, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false)
	Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first.
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.
void	getRobotMarkers (visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, bool include_attached=false)
	Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first.
const RobotModelConstPtr &	getRobotModel () const
	Get the robot model this state is constructed for.
std::string	getStateTreeString (const std::string &prefix="") const
std::size_t	getVariableCount () const
	Get the number of variables that make up this state.
const std::vector< std::string > &	getVariableNames () const
	Get the names of the variables that make up this state, in the order they are stored in memory.
bool	knowsFrameTransform (const std::string &id) const
	Check if a transformation matrix from the model frame to frame id is known.
RobotState &	operator= (const RobotState &other)
	Copy operator.
void	printDirtyInfo (std::ostream &out=std::cout) const
void	printStateInfo (std::ostream &out=std::cout) const
void	printStatePositions (std::ostream &out=std::cout) const
void	printTransform (const Eigen::Affine3d &transform, std::ostream &out=std::cout) const
void	printTransforms (std::ostream &out=std::cout) const
	RobotState (const RobotModelConstPtr &robot_model)
	A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information.
	RobotState (const RobotState &other)
	Copy constructor.
	~RobotState ()
Getting and setting variable position
double *	getVariablePositions ()
	Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
const double *	getVariablePositions () const
	Get a raw pointer to the positions of the variables stored in this state.
void	setVariablePositions (const double *position)
	It is assumed positions is an array containing the new positions for all variables in this state. Those values are copied into the state.
void	setVariablePositions (const std::vector< double > &position)
	It is assumed positions is an array containing the new positions for all variables in this state. Those values are copied into the state.
void	setVariablePositions (const std::map< std::string, double > &variable_map)
	Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariablePositions (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables)
	Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
void	setVariablePositions (const std::vector< std::string > &variable_names, const std::vector< double > &variable_position)
	Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
void	setVariablePosition (const std::string &variable, double value)
	Set the position of a single variable. An exception is thrown if the variable name is not known.
void	setVariablePosition (int index, double value)
	Set the position of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
double	getVariablePosition (const std::string &variable) const
	Get the position of a particular variable. An exception is thrown if the variable is not known.
double	getVariablePosition (int index) const
	Get the position of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Getting and setting variable velocity
bool	hasVelocities () const
	By default, if velocities are never set or initialized, the state remembers that there are no velocities set. This is useful to know when serializing or copying the state.
double *	getVariableVelocities ()
	Get raw access to the velocities of the variables that make up this state. The values are in the same order as reported by getVariableNames()
const double *	getVariableVelocities () const
	Get const access to the velocities of the variables that make up this state. The values are in the same order as reported by getVariableNames()
void	setVariableVelocities (const double *velocity)
	Given an array with velocity values for all variables, set those values as the velocities in this state.
void	setVariableVelocities (const std::vector< double > &velocity)
	Given an array with velocity values for all variables, set those values as the velocities in this state.
void	setVariableVelocities (const std::map< std::string, double > &variable_map)
	Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariableVelocities (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables)
	Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
void	setVariableVelocities (const std::vector< std::string > &variable_names, const std::vector< double > &variable_velocity)
	Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariableVelocity (const std::string &variable, double value)
	Set the velocity of a variable. If an unknown variable name is specified, an exception is thrown.
void	setVariableVelocity (int index, double value)
	Set the velocity of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
double	getVariableVelocity (const std::string &variable) const
	Get the velocity of a particular variable. An exception is thrown if the variable is not known.
double	getVariableVelocity (int index) const
	Get the velocity of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Getting and setting variable acceleration
bool	hasAccelerations () const
	By default, if accelerations are never set or initialized, the state remembers that there are no accelerations set. This is useful to know when serializing or copying the state. If hasAccelerations() reports true, hasEffort() will certainly report false.
double *	getVariableAccelerations ()
	Get raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames(). The area of memory overlaps with effort (effort and acceleration should not be set at the same time)
const double *	getVariableAccelerations () const
	Get const raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames()
void	setVariableAccelerations (const double *acceleration)
	Given an array with acceleration values for all variables, set those values as the accelerations in this state.
void	setVariableAccelerations (const std::vector< double > &acceleration)
	Given an array with acceleration values for all variables, set those values as the accelerations in this state.
void	setVariableAccelerations (const std::map< std::string, double > &variable_map)
	Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariableAccelerations (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables)
	Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
void	setVariableAccelerations (const std::vector< std::string > &variable_names, const std::vector< double > &variable_acceleration)
	Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariableAcceleration (const std::string &variable, double value)
	Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown.
void	setVariableAcceleration (int index, double value)
	Set the acceleration of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
double	getVariableAcceleration (const std::string &variable) const
	Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
double	getVariableAcceleration (int index) const
	Get the acceleration of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Getting and setting variable effort
bool	hasEffort () const
	By default, if effort is never set or initialized, the state remembers that there is no effort set. This is useful to know when serializing or copying the state. If hasEffort() reports true, hasAccelerations() will certainly report false.
double *	getVariableEffort ()
	Get raw access to the effort of the variables that make up this state. The values are in the same order as reported by getVariableNames(). The area of memory overlaps with accelerations (effort and acceleration should not be set at the same time)
const double *	getVariableEffort () const
	Get const raw access to the effort of the variables that make up this state. The values are in the same order as reported by getVariableNames().
void	setVariableEffort (const double *effort)
	Given an array with effort values for all variables, set those values as the effort in this state.
void	setVariableEffort (const std::vector< double > &effort)
	Given an array with effort values for all variables, set those values as the effort in this state.
void	setVariableEffort (const std::map< std::string, double > &variable_map)
	Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariableEffort (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables)
	Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
void	setVariableEffort (const std::vector< std::string > &variable_names, const std::vector< double > &variable_acceleration)
	Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown.
void	setVariableEffort (const std::string &variable, double value)
	Set the effort of a variable. If an unknown variable name is specified, an exception is thrown.
void	setVariableEffort (int index, double value)
	Set the effort of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
double	getVariableEffort (const std::string &variable) const
	Get the effort of a particular variable. An exception is thrown if the variable is not known.
double	getVariableEffort (int index) const
	Get the effort of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Getting and setting joint positions, velocities, accelerations and effort
void	setJointPositions (const std::string &joint_name, const double *position)
void	setJointPositions (const std::string &joint_name, const std::vector< double > &position)
void	setJointPositions (const JointModel *joint, const std::vector< double > &position)
void	setJointPositions (const JointModel joint, const double position)
void	setJointPositions (const std::string &joint_name, const Eigen::Affine3d &transform)
void	setJointPositions (const JointModel *joint, const Eigen::Affine3d &transform)
void	setJointVelocities (const JointModel joint, const double velocity)
void	setJointEfforts (const JointModel joint, const double effort)
const double *	getJointPositions (const std::string &joint_name) const
const double *	getJointPositions (const JointModel *joint) const
const double *	getJointVelocities (const std::string &joint_name) const
const double *	getJointVelocities (const JointModel *joint) const
const double *	getJointAccelerations (const std::string &joint_name) const
const double *	getJointAccelerations (const JointModel *joint) const
const double *	getJointEffort (const std::string &joint_name) const
const double *	getJointEffort (const JointModel *joint) const
Getting and setting group positions
void	setJointGroupPositions (const std::string &joint_group_name, const double *gstate)
	Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupPositions (const std::string &joint_group_name, const std::vector< double > &gstate)
	Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupPositions (const JointModelGroup *group, const std::vector< double > &gstate)
	Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupPositions (const JointModelGroup group, const double gstate)
	Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupPositions (const std::string &joint_group_name, const Eigen::VectorXd &values)
	Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupPositions (const JointModelGroup *group, const Eigen::VectorXd &values)
	Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	copyJointGroupPositions (const std::string &joint_group_name, std::vector< double > &gstate) const
	For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupPositions (const std::string &joint_group_name, double *gstate) const
	For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupPositions (const JointModelGroup *group, std::vector< double > &gstate) const
	For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupPositions (const JointModelGroup group, double gstate) const
	For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupPositions (const std::string &joint_group_name, Eigen::VectorXd &values) const
	For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupPositions (const JointModelGroup *group, Eigen::VectorXd &values) const
	For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Getting and setting group velocities
void	setJointGroupVelocities (const std::string &joint_group_name, const double *gstate)
	Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupVelocities (const std::string &joint_group_name, const std::vector< double > &gstate)
	Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupVelocities (const JointModelGroup *group, const std::vector< double > &gstate)
	Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupVelocities (const JointModelGroup group, const double gstate)
	Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupVelocities (const std::string &joint_group_name, const Eigen::VectorXd &values)
	Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupVelocities (const JointModelGroup *group, const Eigen::VectorXd &values)
	Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	copyJointGroupVelocities (const std::string &joint_group_name, std::vector< double > &gstate) const
	For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupVelocities (const std::string &joint_group_name, double *gstate) const
	For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupVelocities (const JointModelGroup *group, std::vector< double > &gstate) const
	For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupVelocities (const JointModelGroup group, double gstate) const
	For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupVelocities (const std::string &joint_group_name, Eigen::VectorXd &values) const
	For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupVelocities (const JointModelGroup *group, Eigen::VectorXd &values) const
	For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Getting and setting group accelerations
void	setJointGroupAccelerations (const std::string &joint_group_name, const double *gstate)
	Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupAccelerations (const std::string &joint_group_name, const std::vector< double > &gstate)
	Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupAccelerations (const JointModelGroup *group, const std::vector< double > &gstate)
	Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupAccelerations (const JointModelGroup group, const double gstate)
	Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupAccelerations (const std::string &joint_group_name, const Eigen::VectorXd &values)
	Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	setJointGroupAccelerations (const JointModelGroup *group, const Eigen::VectorXd &values)
	Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
void	copyJointGroupAccelerations (const std::string &joint_group_name, std::vector< double > &gstate) const
	For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupAccelerations (const std::string &joint_group_name, double *gstate) const
	For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupAccelerations (const JointModelGroup *group, std::vector< double > &gstate) const
	For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupAccelerations (const JointModelGroup group, double gstate) const
	For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupAccelerations (const std::string &joint_group_name, Eigen::VectorXd &values) const
	For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
void	copyJointGroupAccelerations (const JointModelGroup *group, Eigen::VectorXd &values) const
	For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Setting from Inverse Kinematics
bool	setToIKSolverFrame (Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
	Convert the frame of reference of the pose to that same frame as the IK solver expects.
bool	setToIKSolverFrame (Eigen::Affine3d &pose, const std::string &ik_frame)
	Convert the frame of reference of the pose to that same frame as the IK solver expects.
bool	setFromIK (const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
bool	setFromIK (const JointModelGroup *group, const geometry_msgs::Pose &pose, const std::string &tip, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
bool	setFromIK (const JointModelGroup *group, const Eigen::Affine3d &pose, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
bool	setFromIK (const JointModelGroup *group, const Eigen::Affine3d &pose, const std::string &tip, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
bool	setFromIK (const JointModelGroup *group, const Eigen::Affine3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
bool	setFromIK (const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success.
bool	setFromIK (const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success.
bool	setFromIKSubgroups (const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for non-chain kinematics. In this case, we divide the group into subgroups and do IK solving individually
bool	setFromDiffIK (const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
	Set the joint values from a cartesian velocity applied during a time dt.
bool	setFromDiffIK (const JointModelGroup *group, const geometry_msgs::Twist &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
	Set the joint values from a cartesian velocity applied during a time dt.
double	computeCartesianPath (const JointModelGroup group, std::vector< RobotStatePtr > &traj, const LinkModel link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group.
double	computeCartesianPath (const JointModelGroup group, std::vector< RobotStatePtr > &traj, const LinkModel link, const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group.
double	computeCartesianPath (const JointModelGroup group, std::vector< RobotStatePtr > &traj, const LinkModel link, const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
	Compute the sequence of joint values that perform a general Cartesian path.
bool	getJacobian (const JointModelGroup group, const LinkModel link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
	Compute the Jacobian with reference to a particular point on a given link, for a specified group.
bool	getJacobian (const JointModelGroup group, const LinkModel link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false)
	Compute the Jacobian with reference to a particular point on a given link, for a specified group.
Eigen::MatrixXd	getJacobian (const JointModelGroup *group, const Eigen::Vector3d &reference_point_position=Eigen::Vector3d(0.0, 0.0, 0.0)) const
	Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an exception is thrown.
Eigen::MatrixXd	getJacobian (const JointModelGroup *group, const Eigen::Vector3d &reference_point_position=Eigen::Vector3d(0.0, 0.0, 0.0))
	Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an exception is thrown.
void	computeVariableVelocity (const JointModelGroup jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel tip) const
	Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot.
void	computeVariableVelocity (const JointModelGroup jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel tip)
	Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot.
bool	integrateVariableVelocity (const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
	Given the velocities for the variables in this group (qdot) and an amount of time (dt), update the current state using the Euler forward method. If the constraint specified is satisfied, return true, otherwise return false.
Getting and setting whole states
void	setVariableValues (const sensor_msgs::JointState &msg)
void	setToDefaultValues ()
	Set all joints to their default positions. The default position is 0, or if that is not within bounds then half way between min and max bound.
bool	setToDefaultValues (const JointModelGroup *group, const std::string &name)
	Set the joints in group to the position name defined in the SRDF.
void	setToRandomPositions ()
	Set all joints to random values. Values will be within default bounds.
void	setToRandomPositions (const JointModelGroup *group)
	Set all joints in group to random values. Values will be within default bounds.
void	setToRandomPositions (const JointModelGroup *group, random_numbers::RandomNumberGenerator &rng)
	Set all joints in group to random values using a specified random number generator. Values will be within default bounds.
void	setToRandomPositionsNearBy (const JointModelGroup *group, const RobotState &near, double distance)
	Set all joints in group to random values near the value in . distance is the maximum amount each joint value will vary from the corresponding value in near. represents meters for prismatic/postitional joints and radians for revolute/orientation joints. Resulting values are clamped within default bounds.
void	setToRandomPositionsNearBy (const JointModelGroup *group, const RobotState &near, const std::vector< double > &distances)
	Set all joints in group to random values near the value in . distances MUST have the same size as `group.getActiveJointModels()`. Each value in distances is the maximum amount the corresponding active joint in group will vary from the corresponding value in near. represents meters for prismatic/postitional joints and radians for revolute/orientation joints. Resulting values are clamped within default bounds.
Updating and getting transforms
void	updateCollisionBodyTransforms ()
	Update the transforms for the collision bodies. This call is needed before calling collision checking. If updating link transforms or joint transorms is needed, the corresponding updates are also triggered.
void	updateLinkTransforms ()
	Update the reference frame transforms for links. This call is needed before using the transforms of links for coordinate transforms.
void	update (bool force=false)
	Update all transforms.
void	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.
void	updateStateWithLinkAt (const LinkModel *link, const Eigen::Affine3d &transform, bool backward=false)
	Update the state after setting a particular link to the input global transform pose.
const Eigen::Affine3d &	getGlobalLinkTransform (const std::string &link_name)
const Eigen::Affine3d &	getGlobalLinkTransform (const LinkModel *link)
const Eigen::Affine3d &	getCollisionBodyTransforms (const std::string &link_name, std::size_t index)
const Eigen::Affine3d &	getCollisionBodyTransform (const LinkModel *link, std::size_t index)
const Eigen::Affine3d &	getJointTransform (const std::string &joint_name)
const Eigen::Affine3d &	getJointTransform (const JointModel *joint)
const Eigen::Affine3d &	getGlobalLinkTransform (const std::string &link_name) const
const Eigen::Affine3d &	getGlobalLinkTransform (const LinkModel *link) const
const Eigen::Affine3d &	getCollisionBodyTransform (const std::string &link_name, std::size_t index) const
const Eigen::Affine3d &	getCollisionBodyTransform (const LinkModel *link, std::size_t index) const
const Eigen::Affine3d &	getJointTransform (const std::string &joint_name) const
const Eigen::Affine3d &	getJointTransform (const JointModel *joint) const
bool	dirtyJointTransform (const JointModel *joint) const
bool	dirtyLinkTransforms () const
bool	dirtyCollisionBodyTransforms () const
bool	dirty () const
	Returns true if anything in this state is dirty.
Computing distances
double	distance (const RobotState &other) const
double	distance (const RobotState &other, const JointModelGroup *joint_group) const
double	distance (const RobotState &other, const JointModel *joint) const
void	interpolate (const RobotState &to, double t, RobotState &state) const
	Interpolate from this state towards state to, at time t in [0,1]. The result is stored in state, mimic joints are correctly updated and flags are set so that FK is recomputed when needed.
void	interpolate (const RobotState &to, double t, RobotState &state, const JointModelGroup *joint_group) const
	Interpolate from this state towards to, at time t in [0,1], but only for the joints in the specified group. If mimic joints need to be updated, they are updated accordingly. Flags are set so that FK computation is triggered when needed.
void	interpolate (const RobotState &to, double t, RobotState &state, const JointModel *joint) const
	Update state by interpolating form this state towards to, at time t in [0,1] but only for the joint joint. If there are joints that mimic this joint, they are updated. Flags are set so that FK computation is triggered as needed.
void	enforceBounds ()
void	enforceBounds (const JointModelGroup *joint_group)
void	enforceBounds (const JointModel *joint)
void	enforcePositionBounds (const JointModel *joint)
void	enforceVelocityBounds (const JointModel *joint)
bool	satisfiesBounds (double margin=0.0) const
bool	satisfiesBounds (const JointModelGroup *joint_group, double margin=0.0) const
bool	satisfiesBounds (const JointModel *joint, double margin=0.0) const
bool	satisfiesPositionBounds (const JointModel *joint, double margin=0.0) const
bool	satisfiesVelocityBounds (const JointModel *joint, double margin=0.0) const
std::pair< double, const JointModel * >	getMinDistanceToPositionBounds () const
	Get the minimm distance from this state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned.
std::pair< double, const JointModel * >	getMinDistanceToPositionBounds (const JointModelGroup *group) const
	Get the minimm distance from a group in this state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned.
std::pair< double, const JointModel * >	getMinDistanceToPositionBounds (const std::vector< const JointModel * > &joints) const
	Get the minimm distance from a set of joints in the state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned.
Managing attached bodies
void	attachBody (AttachedBody *attached_body)
	Add an attached body to this state. Ownership of the memory for the attached body is assumed by the 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 trajectory_msgs::JointTrajectory &detach_posture=trajectory_msgs::JointTrajectory())
	Add an attached 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 trajectory_msgs::JointTrajectory &detach_posture=trajectory_msgs::JointTrajectory())
	Add an attached body to a link.
void	getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies) const
	Get all bodies attached to the model corresponding to this state.
void	getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies, const JointModelGroup *lm) const
	Get all bodies attached to a particular group the model corresponding to this state.
void	getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies, const LinkModel *lm) const
	Get all bodies attached to a particular link in the model corresponding to this state.
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	clearAttachedBodies (const LinkModel *link)
	Clear the bodies attached to a specific link.
void	clearAttachedBodies (const JointModelGroup *group)
	Clear the bodies attached to a specific group.
void	clearAttachedBodies ()
	Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
const AttachedBody *	getAttachedBody (const std::string &name) const
	Get the attached body named name. Return NULL if not found.
bool	hasAttachedBody (const std::string &id) const
	Check if an attached body named id exists in this state.
void	setAttachedBodyUpdateCallback (const AttachedBodyCallback &callback)
Private Member Functions
void	allocMemory ()
bool	checkCollisionTransforms () const
	This function is only called in debug mode.
bool	checkJointTransforms (const JointModel *joint) const
	This function is only called in debug mode.
bool	checkLinkTransforms () const
	This function is only called in debug mode.
void	copyFrom (const RobotState &other)
void	getMissingKeys (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) const
void	getStateTreeJointString (std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
void	markAcceleration ()
void	markDirtyJointTransforms (const JointModel *joint)
void	markDirtyJointTransforms (const JointModelGroup *group)
void	markEffort ()
void	markVelocity ()
void	updateLinkTransformsInternal (const JointModel *start)
void	updateMimicJoint (const JointModel *joint)
void	updateMimicJoint (const std::vector< const JointModel * > &mim)
	Update a set of joints that are certain to be mimicking other joints.
Private Attributes
double *	acceleration_
std::map< std::string, AttachedBody * >	attached_body_map_
	All attached bodies that are part of this state, indexed by their name.
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.
const JointModel *	dirty_collision_body_transforms_
unsigned char *	dirty_joint_transforms_
const JointModel *	dirty_link_transforms_
double *	effort_
Eigen::Affine3d *	global_collision_body_transforms_
Eigen::Affine3d *	global_link_transforms_
bool	has_acceleration_
bool	has_effort_
bool	has_velocity_
void *	memory_
double *	position_
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.
RobotModelConstPtr	robot_model_
Eigen::Affine3d *	variable_joint_transforms_
double *	velocity_

Detailed Description

Representation of a robot's state. This includes position, velocity, acceleration and effort.

At the lowest level, a state is a collection of variables. Each variable has a name and can have position, velocity, acceleration and effort associated to it. Effort and acceleration share the memory area for efficiency reasons (one should not set both acceleration and effort in the same state and expect things to work). Often variables correspond to joint names as well (joints with one degree of freedom have one variable), but joints with multiple degrees of freedom have more variables. Operations are allowed at variable level, joint level (see JointModel) and joint group level (see JointModelGroup).

For efficiency reasons a state computes forward kinematics in a lazy fashion. This can sometimes lead to problems if the update() function was not called on the state.

Definition at line 82 of file robot_state.h.

Constructor & Destructor Documentation

moveit::core::RobotState::RobotState ( const RobotModelConstPtr & robot_model )

A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information.

Definition at line 46 of file robot_state.cpp.

moveit::core::RobotState::~RobotState ( )

Definition at line 70 of file robot_state.cpp.

moveit::core::RobotState::RobotState ( const RobotState & other )

Copy constructor.

Definition at line 63 of file robot_state.cpp.

Member Function Documentation

void moveit::core::RobotState::allocMemory ( void ) [private]

Definition at line 78 of file robot_state.cpp.

void moveit::core::RobotState::attachBody ( AttachedBody * attached_body )

Add an attached body to this state. Ownership of the memory for the attached body is assumed by the state.

This only adds the given body to this RobotState instance. It does not change anything about other representations of the object elsewhere in the system. So if the body represents an object in a collision_detection::World (like from a planning_scene::PlanningScene), you will likely need to remove the corresponding object from that world to avoid having collisions detected against it.

Note:: This version of the function (taking an AttachedBody pointer) does not copy the AttachedBody object, it just uses it directly. The AttachedBody object stores its position data internally. This means you should never attach a single AttachedBody instance to multiple RobotState instances, or the body positions will get corrupted. You need to make a fresh copy of the AttachedBody object for each RobotState you attach it to.

Definition at line 839 of file robot_state.cpp.

void moveit::core::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 trajectory_msgs::JointTrajectory &	detach_posture = `trajectory_msgs::JointTrajectory()`
	)

Add an attached body to a link.

Parameters:

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

This only adds the given body to this RobotState instance. It does not change anything about other representations of the object elsewhere in the system. So if the body represents an object in a collision_detection::World (like from a planning_scene::PlanningScene), you will likely need to remove the corresponding object from that world to avoid having collisions detected against it.

Definition at line 847 of file robot_state.cpp.

void moveit::core::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 trajectory_msgs::JointTrajectory &	detach_posture = `trajectory_msgs::JointTrajectory()`
	)		`[inline]`

Add an attached body to a link.

Parameters:

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

This only adds the given body to this RobotState instance. It does not change anything about other representations of the object elsewhere in the system. So if the body represents an object in a collision_detection::World (like from a planning_scene::PlanningScene), you will likely need to remove the corresponding object from that world to avoid having collisions detected against it.

Definition at line 1550 of file robot_state.h.

bool moveit::core::RobotState::checkCollisionTransforms ( ) const [private]

This function is only called in debug mode.

Definition at line 172 of file robot_state.cpp.

bool moveit::core::RobotState::checkJointTransforms ( const JointModel * joint ) const [private]

This function is only called in debug mode.

Definition at line 152 of file robot_state.cpp.

bool moveit::core::RobotState::checkLinkTransforms ( ) const [private]

This function is only called in debug mode.

Definition at line 162 of file robot_state.cpp.

void moveit::core::RobotState::clearAttachedBodies ( const LinkModel * link )

Clear the bodies attached to a specific link.

Definition at line 901 of file robot_state.cpp.

void moveit::core::RobotState::clearAttachedBodies ( const JointModelGroup * group )

Clear the bodies attached to a specific group.

Definition at line 919 of file robot_state.cpp.

void moveit::core::RobotState::clearAttachedBodies ( )

Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.

Definition at line 889 of file robot_state.cpp.

bool moveit::core::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 937 of file robot_state.cpp.

void moveit::core::RobotState::computeAABB ( std::vector< double > & aabb ) const

Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz)

Definition at line 2024 of file robot_state.cpp.

void moveit::core::RobotState::computeAABB ( std::vector< double > & aabb ) [inline]

Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz)

Definition at line 1596 of file robot_state.h.

double moveit::core::RobotState::computeCartesianPath	(	const JointModelGroup *	group,
		std::vector< RobotStatePtr > &	traj,
		const LinkModel *	link,
		const Eigen::Vector3d &	direction,
		bool	global_reference_frame,
		double	distance,
		double	max_step,
		double	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)

Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group.

The Cartesian path to be followed is specified as a direction of motion (direction, unit vector) for the origin of a robot link (link). The direction is assumed to be either in a global reference frame or in the local reference frame of the link. In the latter case (global_reference_frame is false) the direction is rotated accordingly. The link needs to move in a straight line, following the specified direction, for the desired distance. The resulting joint values are stored in the vector traj, one by one. The maximum distance in Cartesian space between consecutive points on the resulting path is specified by max_step. If a validCallback is specified, this is passed to the internal call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to the distance that was computed and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose. During the computation of the trajectory, it is sometimes preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is as expected. To account for this, the jump_threshold parameter is provided. As the joint values corresponding to the Cartesian path are computed, distances in joint space between consecutive points are also computed. Once the sequence of joint values is computed, the average distance between consecutive points (in joint space) is also computed. It is then verified that none of the computed distances is above the average distance by a factor larger than jump_threshold. If a point in joint is found such that it is further away than the previous one by more than average_consecutive_distance * jump_threshold, that is considered a failure and the returned path is truncated up to just before the jump. The jump detection can be disabled by setting jump_threshold to 0.0

Definition at line 1857 of file robot_state.cpp.

double moveit::core::RobotState::computeCartesianPath	(	const JointModelGroup *	group,
		std::vector< RobotStatePtr > &	traj,
		const LinkModel *	link,
		const Eigen::Affine3d &	target,
		bool	global_reference_frame,
		double	max_step,
		double	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)

Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group.

The Cartesian path to be followed is specified as a target frame to be reached (target) for the origin of a robot link (link). The target frame is assumed to be either in a global reference frame or in the local reference frame of the link. In the latter case (global_reference_frame is false) the target is rotated accordingly. The link needs to move in a straight line towards the target. The resulting joint values are stored in the vector traj, one by one. The maximum distance in Cartesian space between consecutive points on the resulting path is specified by max_step. If a validCallback is specified, this is passed to the internal call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to the percentage of the path (between 0 and 1) that was completed and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose. During the computation of the trajectory, it is sometimes preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is as expected. To account for this, the jump_threshold parameter is provided. As the joint values corresponding to the Cartesian path are computed, distances in joint space between consecutive points are also computed. Once the sequence of joint values is computed, the average distance between consecutive points (in joint space) is also computed. It is then verified that none of the computed distances is above the average distance by a factor larger than jump_threshold. If a point in joint is found such that it is further away than the previous one by more than average_consecutive_distance * jump_threshold, that is considered a failure and the returned path is truncated up to just before the jump. The jump detection can be disabled by setting jump_threshold to 0.0

Definition at line 1879 of file robot_state.cpp.

double moveit::core::RobotState::computeCartesianPath	(	const JointModelGroup *	group,
		std::vector< RobotStatePtr > &	traj,
		const LinkModel *	link,
		const EigenSTL::vector_Affine3d &	waypoints,
		bool	global_reference_frame,
		double	max_step,
		double	jump_threshold,
		const GroupStateValidityCallbackFn &	validCallback = `GroupStateValidityCallbackFn()`,
		const kinematics::KinematicsQueryOptions &	options = `kinematics::KinematicsQueryOptions()`
	)

Compute the sequence of joint values that perform a general Cartesian path.

The Cartesian path to be followed is specified as a set of waypoints to be sequentially reached for the origin of a robot link (link). The waypoints are transforms given either in a global reference frame or in the local reference frame of the link at the immediately preceeding waypoint. The link needs to move in a straight line between two consecutive waypoints. The resulting joint values are stored in the vector traj, one by one. The maximum distance in Cartesian space between consecutive points on the resulting path is specified by max_step. If a validCallback is specified, this is passed to the internal call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to the percentage of the path (between 0 and 1) that was completed and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose. During the computation of the trajectory, it is sometimes preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is as expected. To account for this, the jump_threshold parameter is provided. As the joint values corresponding to the Cartesian path are computed, distances in joint space between consecutive points are also computed. Once the sequence of joint values is computed, the average distance between consecutive points (in joint space) is also computed. It is then verified that none of the computed distances is above the average distance by a factor larger than jump_threshold. If a point in joint is found such that it is further away than the previous one by more than average_consecutive_distance * jump_threshold, that is considered a failure and the returned path is truncated up to just before the jump. The jump detection can be disabled by setting jump_threshold to 0.0

Definition at line 1953 of file robot_state.cpp.

void moveit::core::RobotState::computeVariableVelocity	(	const JointModelGroup *	jmg,
		Eigen::VectorXd &	qdot,
		const Eigen::VectorXd &	twist,
		const LinkModel *	tip
	)		const

Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot.

Definition at line 1218 of file robot_state.cpp.

void moveit::core::RobotState::computeVariableVelocity	(	const JointModelGroup *	jmg,
		Eigen::VectorXd &	qdot,
		const Eigen::VectorXd &	twist,
		const LinkModel *	tip
	)		`[inline]`

Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot.

Definition at line 1220 of file robot_state.h.

void moveit::core::RobotState::copyFrom ( const RobotState & other ) [private]

Definition at line 108 of file robot_state.cpp.

void moveit::core::RobotState::copyJointGroupAccelerations	(	const std::string &	joint_group_name,
		std::vector< double > &	gstate
	)		const `[inline]`