Public Member Functions | Private Member Functions | Private Attributes
robot_state::JointStateGroup Class Reference

The joint state corresponding to a group. More...

#include <joint_state_group.h>

List of all members.

Public Member Functions

bool avoidJointLimitsSecondaryTask (const JointStateGroup *joint_state_group, Eigen::VectorXd &stvector, double activation_threshold, double gain) const
 Secondary task that tries to keep away from joint limits THIS FUNCTION NEEDS TO MOVE ELSEWHERE.
double computeCartesianPath (std::vector< boost::shared_ptr< RobotState > > &traj, const std::string &link_name, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold, const StateValidityCallbackFn &validCallback=StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
 Compute the sequence of joint values that correspond to a straight Cartesian path.
double computeCartesianPath (std::vector< boost::shared_ptr< RobotState > > &traj, const std::string &link_name, const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold, const StateValidityCallbackFn &validCallback=StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
 Compute the sequence of joint values that correspond to a straight Cartesian path.
double computeCartesianPath (std::vector< boost::shared_ptr< RobotState > > &traj, const std::string &link_name, const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold, const StateValidityCallbackFn &validCallback=StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
 Compute the sequence of joint values that perform a general Cartesian path.
void computeJointVelocity (Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const std::string &tip, const SecondaryTaskFn &st=SecondaryTaskFn()) const
 Given a twist for a particular link (tip), and an optional secondary task (st), compute the corresponding joint velocity and store it in qdot.
double distance (const JointStateGroup *other) const
void enforceBounds ()
 Force the joint to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi.
unsigned getDefaultIKAttempts () const
 Get the default number of ik attempts.
double getDefaultIKTimeout () const
 Get the default IK timeout.
bool getJacobian (const std::string &link_name, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaterion_representation=false) const
 Given a set of joint angles, compute the jacobian with reference to a particular point on a given link.
const
robot_model::JointModelGroup
getJointModelGroup () const
 Get the joint model corresponding to this joint state group.
const std::vector< std::string > & getJointNames () const
 Get the joint names corresponding to this joint state.
const std::vector< JointState * > & getJointRoots () const
 Get the state corresponding to root joints in this group.
JointStategetJointState (const std::string &joint) const
 Get a joint state by its name.
const std::vector< JointState * > & getJointStateVector () const
 Get the vector of joint state for this group.
std::pair< double, int > getMinDistanceToBounds () const
 Returns the minimum distance of a joint to the joint limits for the group. Will not consider planar joints if they are not bounded in all DOFs. If planar joints are bounded, this function will use the distance function defined for planar joints. This function will not consider floating joints at all.
const std::string & getName () const
 Get the name of the joint model group corresponding to this joint state.
random_numbers::RandomNumberGeneratorgetRandomNumberGenerator ()
 Return the instance of a random number generator.
const RobotStategetRobotState () const
 Get the kinematic state this link is part of.
RobotStategetRobotState ()
 Get the kinematic state this link is part of.
unsigned int getVariableCount () const
 Get the number of (active) DOFs for the joint model group corresponding to this state.
void getVariableValues (std::vector< double > &joint_state_values) const
 Get current joint values.
void getVariableValues (Eigen::VectorXd &joint_state_values) const
 Get current joint values.
void getVariableValues (std::map< std::string, double > &joint_state_values) const
 Get a map between variable names and joint state values.
bool hasJointState (const std::string &joint) const
 Check if a joint is part of this group.
double infinityNormDistance (const JointStateGroup *other) const
 Get the infinity norm distance between two joint states.
bool integrateJointVelocity (const Eigen::VectorXd &qdot, double dt, const StateValidityCallbackFn &constraint=StateValidityCallbackFn())
 Given the velocities for the joints 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.
void interpolate (const JointStateGroup *to, const double t, JointStateGroup *dest) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW JointStateGroup (RobotState *state, const robot_model::JointModelGroup *jmg)
 Default constructor.
JointStateGroupoperator= (const JointStateGroup &other)
bool satisfiesBounds (double margin=0.0) const
 Checks if the current joint state values are all within the bounds set in the model.
bool setFromDiffIK (const Eigen::VectorXd &twist, const std::string &tip, double dt, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), const SecondaryTaskFn &st=SecondaryTaskFn())
 Set the joint values from a cartesian velocity applied during a time dt.
bool setFromDiffIK (const geometry_msgs::Twist &twist, const std::string &tip, double dt, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), const SecondaryTaskFn &st=SecondaryTaskFn())
 Set the joint values from a cartesian velocity applied during a time dt.
bool setFromIK (const geometry_msgs::Pose &pose, const std::string &tip, unsigned int attempts=0, double timeout=0.0, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 geometry_msgs::Pose &pose, unsigned int attempts=0, double timeout=0.0, 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 geometry_msgs::Pose &pose, unsigned int attempts=0, double timeout=0.0, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 Eigen::Affine3d &pose, unsigned int attempts=0, double timeout=0.0, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 Eigen::Affine3d &pose_in, const std::string &tip_in, unsigned int attempts, double timeout, 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 Eigen::Affine3d &pose_in, unsigned int attempts, double timeout, const kinematics::KinematicsQueryOptions &options)
 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 Eigen::Affine3d &pose, const std::string &tip, unsigned int attempts=0, double timeout=0.0, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 Eigen::Affine3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, unsigned int attempts=0, double timeout=0.0, const StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 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 StateValidityCallbackFn &constraint=StateValidityCallbackFn(), 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 setToDefaultState (const std::string &name)
 Set the group to a named default state. Return false on failure.
void setToDefaultValues ()
 Bring the group to a default state. All joints are at 0. If 0 is not within the bounds of the joint, the middle of the bounds is used.
void setToRandomValues ()
 Sample a random state in accordance with the type of joints employed.
void setToRandomValuesNearBy (const std::vector< double > &near, const std::map< robot_model::JointModel::JointType, double > &distance_map)
 Sample a random state in accordance with the type of joints employed, near the specified joint state. The distance map specifies distances according to joint type.
void setToRandomValuesNearBy (const std::vector< double > &near, const std::vector< double > &distances)
 Sample a random state in accordance with the type of joints employed, near the specified joint state. The distances vector specifies a distance for each joint model. The distance computation uses an InfinityNorm computation.
bool setVariableValues (const std::vector< double > &joint_state_values)
 Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.
bool setVariableValues (const Eigen::VectorXd &joint_state_values)
 Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.
void setVariableValues (const std::map< std::string, double > &joint_state_map)
 Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.
void setVariableValues (const sensor_msgs::JointState &js)
 Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.
void updateLinkTransforms ()
 ~JointStateGroup ()

Private Member Functions

void copyFrom (const JointStateGroup &other_jsg)
 Copy the values from another joint state group.
void ikCallbackFnAdapter (const StateValidityCallbackFn &constraint, const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_sol, moveit_msgs::MoveItErrorCodes &error_code)
 This function converts output from the IK plugin to the proper ordering of values expected by this group and passes it to constraint.

Private Attributes

const
robot_model::JointModelGroup
joint_model_group_
 The model of the group that corresponds to this state.
std::vector< JointState * > joint_roots_
 The list of joints that are roots in this group.
std::map< std::string,
JointState * > 
joint_state_map_
 A map from joint names to their instances.
std::vector< JointState * > joint_state_vector_
 Joint instances in the order they appear in the group state.
RobotStatekinematic_state_
 The kinematic state this group is part of.
boost::scoped_ptr
< random_numbers::RandomNumberGenerator
rng_
 For certain operations a group 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.
std::vector< LinkState * > updated_links_
 The list of links that are updated when computeTransforms() is called, in the order they are updated.

Detailed Description

The joint state corresponding to a group.

Definition at line 62 of file joint_state_group.h.


Constructor & Destructor Documentation

Default constructor.

Parameters:
stateA pointer to the kinematic state
jmgThe joint model group corresponding to this joint state

Definition at line 44 of file joint_state_group.cpp.

Definition at line 73 of file joint_state_group.cpp.


Member Function Documentation

bool robot_state::JointStateGroup::avoidJointLimitsSecondaryTask ( const JointStateGroup joint_state_group,
Eigen::VectorXd &  stvector,
double  activation_threshold,
double  gain 
) const

Secondary task that tries to keep away from joint limits THIS FUNCTION NEEDS TO MOVE ELSEWHERE.

Parameters:
joint_state_groupthe joint state group for which to compute the task
stvectorthe output of the function: a vector with joint velocities
activation_thresholdA percentage of the range from which the task is activated, i.e. activate if q > qmax - range * threshold. Typically between 0 and 0.5
gaina gain for this task, multiplies the output velocities

Definition at line 788 of file joint_state_group.cpp.

double robot_state::JointStateGroup::computeCartesianPath ( std::vector< boost::shared_ptr< RobotState > > &  traj,
const std::string &  link_name,
const Eigen::Vector3d &  direction,
bool  global_reference_frame,
double  distance,
double  max_step,
double  jump_threshold,
const StateValidityCallbackFn validCallback = StateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
)

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

The Cartesian path to be followed is specified as a direction of motion (direction, unit vector) for the origin of a robot link (link_name). 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

double robot_state::JointStateGroup::computeCartesianPath ( std::vector< boost::shared_ptr< RobotState > > &  traj,
const std::string &  link_name,
const Eigen::Affine3d &  target,
bool  global_reference_frame,
double  max_step,
double  jump_threshold,
const StateValidityCallbackFn validCallback = StateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions options = kinematics::KinematicsQueryOptions() 
)

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

The Cartesian path to be followed is specified as a target frame to be reached (target) for the origin of a robot link (link_name). 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

double robot_state::JointStateGroup::computeCartesianPath ( std::vector< boost::shared_ptr< RobotState > > &  traj,
const std::string &  link_name,
const EigenSTL::vector_Affine3d waypoints,
bool  global_reference_frame,
double  max_step,
double  jump_threshold,
const StateValidityCallbackFn validCallback = StateValidityCallbackFn(),
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_name). 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

void robot_state::JointStateGroup::computeJointVelocity ( Eigen::VectorXd &  qdot,
const Eigen::VectorXd &  twist,
const std::string &  tip,
const SecondaryTaskFn st = SecondaryTaskFn() 
) const

Given a twist for a particular link (tip), and an optional secondary task (st), compute the corresponding joint velocity and store it in qdot.

Definition at line 710 of file joint_state_group.cpp.

void robot_state::JointStateGroup::copyFrom ( const JointStateGroup other_jsg) [private]

Copy the values from another joint state group.

Definition at line 148 of file joint_state_group.cpp.

Definition at line 250 of file joint_state_group.cpp.

Force the joint to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi.

Definition at line 229 of file joint_state_group.cpp.

Get the default number of ik attempts.

Definition at line 232 of file joint_state_group.h.

Get the default IK timeout.

Definition at line 226 of file joint_state_group.h.

bool robot_state::JointStateGroup::getJacobian ( const std::string &  link_name,
const Eigen::Vector3d &  reference_point_position,
Eigen::MatrixXd &  jacobian,
bool  use_quaterion_representation = false 
) const

Given a set of joint angles, compute the jacobian with reference to a particular point on a given link.

Parameters:
link_nameThe name of the link
reference_point_positionThe reference point position (with respect to the link specified in link_name)
jacobianThe resultant jacobian
use_quaternion_representationFlag indicating if the Jacobian should use a quaternion representation (default is false)
Returns:
True if jacobian was successfully computed, false otherwise

Definition at line 995 of file joint_state_group.cpp.

Get the joint model corresponding to this joint state group.

Definition at line 89 of file joint_state_group.h.

const std::vector<std::string>& robot_state::JointStateGroup::getJointNames ( ) const [inline]

Get the joint names corresponding to this joint state.

Definition at line 201 of file joint_state_group.h.

const std::vector<JointState*>& robot_state::JointStateGroup::getJointRoots ( ) const [inline]

Get the state corresponding to root joints in this group.

Definition at line 195 of file joint_state_group.h.

Get a joint state by its name.

Definition at line 277 of file joint_state_group.cpp.

const std::vector<JointState*>& robot_state::JointStateGroup::getJointStateVector ( ) const [inline]

Get the vector of joint state for this group.

Definition at line 207 of file joint_state_group.h.

std::pair< double, int > robot_state::JointStateGroup::getMinDistanceToBounds ( ) const

Returns the minimum distance of a joint to the joint limits for the group. Will not consider planar joints if they are not bounded in all DOFs. If planar joints are bounded, this function will use the distance function defined for planar joints. This function will not consider floating joints at all.

Returns:
A std::pair with the required distance and index of joint corresponding to distance

Definition at line 1097 of file joint_state_group.cpp.

const std::string& robot_state::JointStateGroup::getName ( ) const [inline]

Get the name of the joint model group corresponding to this joint state.

Definition at line 95 of file joint_state_group.h.

Return the instance of a random number generator.

Definition at line 77 of file joint_state_group.cpp.

Get the kinematic state this link is part of.

Definition at line 77 of file joint_state_group.h.

Get the kinematic state this link is part of.

Definition at line 83 of file joint_state_group.h.

unsigned int robot_state::JointStateGroup::getVariableCount ( ) const [inline]

Get the number of (active) DOFs for the joint model group corresponding to this state.

Definition at line 101 of file joint_state_group.h.

void robot_state::JointStateGroup::getVariableValues ( std::vector< double > &  joint_state_values) const

Get current joint values.

Definition at line 199 of file joint_state_group.cpp.

void robot_state::JointStateGroup::getVariableValues ( Eigen::VectorXd &  joint_state_values) const

Get current joint values.

Definition at line 209 of file joint_state_group.cpp.

void robot_state::JointStateGroup::getVariableValues ( std::map< std::string, double > &  joint_state_values) const

Get a map between variable names and joint state values.

Definition at line 265 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::hasJointState ( const std::string &  joint) const

Check if a joint is part of this group.

Definition at line 84 of file joint_state_group.cpp.

void robot_state::JointStateGroup::ikCallbackFnAdapter ( const StateValidityCallbackFn constraint,
const geometry_msgs::Pose &  ik_pose,
const std::vector< double > &  ik_sol,
moveit_msgs::MoveItErrorCodes &  error_code 
) [private]

This function converts output from the IK plugin to the proper ordering of values expected by this group and passes it to constraint.

Definition at line 982 of file joint_state_group.cpp.

Get the infinity norm distance between two joint states.

Definition at line 236 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::integrateJointVelocity ( const Eigen::VectorXd &  qdot,
double  dt,
const StateValidityCallbackFn constraint = StateValidityCallbackFn() 
)

Given the velocities for the joints 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.

Definition at line 770 of file joint_state_group.cpp.

void robot_state::JointStateGroup::interpolate ( const JointStateGroup to,
const double  t,
JointStateGroup dest 
) const

Definition at line 258 of file joint_state_group.cpp.

robot_state::JointStateGroup & robot_state::JointStateGroup::operator= ( const JointStateGroup other)

Definition at line 141 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::satisfiesBounds ( double  margin = 0.0) const

Checks if the current joint state values are all within the bounds set in the model.

Definition at line 221 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromDiffIK ( const Eigen::VectorXd &  twist,
const std::string &  tip,
double  dt,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
const SecondaryTaskFn st = SecondaryTaskFn() 
)

Set the joint values from a cartesian velocity applied during a time dt.

Parameters:
twista cartesian velocity on the 'tip' frame
tipthe frame for which the twist is given
dta time interval (seconds)
sta secondary task computation function

Definition at line 756 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromDiffIK ( const geometry_msgs::Twist &  twist,
const std::string &  tip,
double  dt,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
const SecondaryTaskFn st = SecondaryTaskFn() 
)

Set the joint values from a cartesian velocity applied during a time dt.

Parameters:
twista cartesian velocity on the 'tip' frame
tipthe frame for which the twist is given
dta time interval (seconds)
sta secondary task computation function

Definition at line 763 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const geometry_msgs::Pose &  pose,
const std::string &  tip,
unsigned int  attempts = 0,
double  timeout = 0.0,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
poseThe pose the tip link in the chain needs to achieve
tipThe name of the link the pose is specified for
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 312 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const geometry_msgs::Pose &  pose,
unsigned int  attempts = 0,
double  timeout = 0.0,
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.

Parameters:
poseThe pose the last link in the chain needs to achieve
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt

Definition at line 300 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const geometry_msgs::Pose &  pose,
unsigned int  attempts = 0,
double  timeout = 0.0,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
poseThe pose the last link in the chain needs to achieve
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 289 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const Eigen::Affine3d &  pose,
unsigned int  attempts = 0,
double  timeout = 0.0,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
poseThe pose the last link in the chain needs to achieve
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 319 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const Eigen::Affine3d &  pose_in,
const std::string &  tip_in,
unsigned int  attempts,
double  timeout,
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.

Parameters:
poseThe pose the last link in the chain needs to achieve
tipThe name of the link the pose is specified for
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt

Definition at line 337 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const Eigen::Affine3d &  pose_in,
unsigned int  attempts,
double  timeout,
const kinematics::KinematicsQueryOptions options 
)

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.

Parameters:
poseThe pose the last link in the chain needs to achieve
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt

Definition at line 344 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const Eigen::Affine3d &  pose,
const std::string &  tip,
unsigned int  attempts = 0,
double  timeout = 0.0,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
poseThe pose the last link in the chain needs to achieve
tipThe name of the frame for which IK is attempted.
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 331 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const Eigen::Affine3d &  pose,
const std::string &  tip,
const std::vector< double > &  consistency_limits,
unsigned int  attempts = 0,
double  timeout = 0.0,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
poseThe pose the last link in the chain needs to achieve
tipThe name of the frame for which IK is attempted.
consistency_limitsThis specifies the desired distance between the solution and the seed state
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 357 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( const EigenSTL::vector_Affine3d poses,
const std::vector< std::string > &  tips,
unsigned int  attempts = 0,
double  timeout = 0.0,
const StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
posesThe poses the last link in each chain needs to achieve
tipsThe names of the frames for which IK is attempted.
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 494 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setFromIK ( 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 StateValidityCallbackFn constraint = StateValidityCallbackFn(),
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.

Parameters:
posesThe poses the last link in each chain needs to achieve
tipsThe names of the frames for which IK is attempted.
consistency_limitsThis specifies the desired distance between the solution and the seed state
attemptsThe number of times IK is attempted
timeoutThe timeout passed to the kinematics solver on each attempt
constraintA state validity constraint to be required for IK solutions

Definition at line 505 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setToDefaultState ( const std::string &  name)

Set the group to a named default state. Return false on failure.

Definition at line 164 of file joint_state_group.cpp.

Bring the group to a default state. All joints are at 0. If 0 is not within the bounds of the joint, the middle of the bounds is used.

Definition at line 156 of file joint_state_group.cpp.

Sample a random state in accordance with the type of joints employed.

Definition at line 173 of file joint_state_group.cpp.

void robot_state::JointStateGroup::setToRandomValuesNearBy ( const std::vector< double > &  near,
const std::map< robot_model::JointModel::JointType, double > &  distance_map 
)

Sample a random state in accordance with the type of joints employed, near the specified joint state. The distance map specifies distances according to joint type.

Definition at line 181 of file joint_state_group.cpp.

void robot_state::JointStateGroup::setToRandomValuesNearBy ( const std::vector< double > &  near,
const std::vector< double > &  distances 
)

Sample a random state in accordance with the type of joints employed, near the specified joint state. The distances vector specifies a distance for each joint model. The distance computation uses an InfinityNorm computation.

Definition at line 190 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setVariableValues ( const std::vector< double > &  joint_state_values)

Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.

Definition at line 89 of file joint_state_group.cpp.

bool robot_state::JointStateGroup::setVariableValues ( const Eigen::VectorXd &  joint_state_values)

Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.

Warning:
Creates a temporary std::vector<double>

Definition at line 112 of file joint_state_group.cpp.

void robot_state::JointStateGroup::setVariableValues ( const std::map< std::string, double > &  joint_state_map)

Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.

Definition at line 120 of file joint_state_group.cpp.

void robot_state::JointStateGroup::setVariableValues ( const sensor_msgs::JointState &  js)

Perform forward kinematics starting at the roots within a group. Links that are not in the group are also updated, but transforms for joints that are not in the group are not recomputed.

Definition at line 127 of file joint_state_group.cpp.

Compute transforms using current joint values

Definition at line 135 of file joint_state_group.cpp.


Member Data Documentation

The model of the group that corresponds to this state.

Definition at line 445 of file joint_state_group.h.

The list of joints that are roots in this group.

Definition at line 454 of file joint_state_group.h.

A map from joint names to their instances.

Definition at line 451 of file joint_state_group.h.

Joint instances in the order they appear in the group state.

Definition at line 448 of file joint_state_group.h.

The kinematic state this group is part of.

Definition at line 442 of file joint_state_group.h.

For certain operations a group 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 463 of file joint_state_group.h.

The list of links that are updated when computeTransforms() is called, in the order they are updated.

Definition at line 457 of file joint_state_group.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