Definition of a joint state - representation of state for a single joint. More...
#include <joint_state.h>
Public Member Functions | |
bool | allVariablesAreDefined (const std::map< std::string, double > &value_map) const |
Specifies whether or not all values associated with a joint are defined in the supplied joint value map. | |
double | distance (const JointState *other) const |
void | enforceBounds () |
Force the joint to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. | |
std::vector< double > & | getAccelerations () |
this will go away; don't use it | |
const std::vector< double > & | getAccelerations () const |
this will go away; don't use it | |
const robot_model::JointModel * | getJointModel () const |
Get the joint model corresponding to this state. | |
const std::string & | getName () const |
Get the name of the model associated with this state. | |
robot_model::JointModel::JointType | getType () const |
Get the type of joint associated with this state. | |
const std::vector< std::pair < double, double > > & | getVariableBounds () const |
Get the variable bounds for this joint, in the same order as the names returned by getVariableNames() | |
unsigned int | getVariableCount () const |
Get the number of variable DOFs for this joint. | |
const std::map< std::string, unsigned int > & | getVariableIndexMap () const |
The set of variables that make up the state value of a joint are stored in some order. This map gives the position of each variable in that order, for each variable name. | |
const std::vector< std::string > & | getVariableNames () const |
Get the required name order for the joint state values. | |
const Eigen::Affine3d & | getVariableTransform () const |
Get the current variable transform. | |
Eigen::Affine3d & | getVariableTransform () |
Get the current variable transform. | |
const std::vector< double > & | getVariableValues () const |
Get the joint state values stored in the required order. | |
std::vector< double > & | getVariableValues () |
Get the joint state values stored in the required order. | |
std::vector< double > & | getVelocities () |
this will go away; don't use it | |
const std::vector< double > & | getVelocities () const |
this will go away; don't use it | |
void | interpolate (const JointState *to, const double t, JointState *dest) const |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | JointState (const robot_model::JointModel *jm) |
Constructs the joint state from the model. | |
JointState (const JointState &other) | |
Copy constructor. | |
JointState & | operator= (const JointState &other) |
Copy the data from one joint state to the 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 | setVariableValue (const std::string &variable, double value) |
Set the value of a particular variable for this joint. | |
void | setVariableValues (const std::map< std::string, double > &value_map) |
Sets the internal values from a map of joint variable names to actual values. | |
void | setVariableValues (const std::map< std::string, double > &value_map, std::vector< std::string > &missing) |
Sets the internal values from a map of joint variable names to actual values. The function also fills the missing vector with the variable names that were not set. | |
bool | setVariableValues (const std::vector< double > &value_vector) |
Sets the internal values from the supplied vector, which are assumed to be in the required order. | |
void | setVariableValues (const double *value_vector) |
Sets the internal values from the supplied array, which are assumed to be in the required order. This function is intended to be fast, does no input checking and should be used carefully. | |
void | setVariableValues (const Eigen::Affine3d &transform) |
Sets the internal values from the transform. | |
void | updateMimicJoints () |
Update the joints that mimic this one. This function is called automatically by the setVariable* functions. | |
~JointState () | |
Destructor. | |
Private Attributes | |
std::vector< double > | horrible_acceleration_placeholder_ |
ignore this | |
std::vector< double > | horrible_velocity_placeholder_ |
ignore this | |
const robot_model::JointModel * | joint_model_ |
The joint model this state corresponds to. | |
std::vector< double > | joint_state_values_ |
The joint values given in the order indicated by joint_variables_index_map_. | |
std::vector< JointState * > | mimic_requests_ |
The set of joints that need to be updated when this one is. | |
Eigen::Affine3d | variable_transform_ |
Tthe local transform (computed by forward kinematics) | |
Friends | |
class | RobotState |
Definition of a joint state - representation of state for a single joint.
Definition at line 48 of file joint_state.h.
robot_state::JointState::JointState | ( | const robot_model::JointModel * | jm | ) |
Constructs the joint state from the model.
Definition at line 39 of file joint_state.cpp.
robot_state::JointState::JointState | ( | const JointState & | other | ) |
Copy constructor.
Definition at line 51 of file joint_state.cpp.
Destructor.
Definition at line 56 of file joint_state.cpp.
bool robot_state::JointState::allVariablesAreDefined | ( | const std::map< std::string, double > & | value_map | ) | const |
Specifies whether or not all values associated with a joint are defined in the supplied joint value map.
Definition at line 197 of file joint_state.cpp.
double robot_state::JointState::distance | ( | const JointState * | other | ) | const [inline] |
Definition at line 106 of file joint_state.h.
Force the joint to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi.
Definition at line 183 of file joint_state.cpp.
std::vector<double>& robot_state::JointState::getAccelerations | ( | ) | [inline] |
this will go away; don't use it
Definition at line 187 of file joint_state.h.
const std::vector<double>& robot_state::JointState::getAccelerations | ( | ) | const [inline] |
this will go away; don't use it
Definition at line 199 of file joint_state.h.
const robot_model::JointModel* robot_state::JointState::getJointModel | ( | ) | const [inline] |
Get the joint model corresponding to this state.
Definition at line 168 of file joint_state.h.
const std::string& robot_state::JointState::getName | ( | ) | const [inline] |
Get the name of the model associated with this state.
Definition at line 114 of file joint_state.h.
robot_model::JointModel::JointType robot_state::JointState::getType | ( | ) | const [inline] |
Get the type of joint associated with this state.
Definition at line 120 of file joint_state.h.
const std::vector<std::pair<double, double> >& robot_state::JointState::getVariableBounds | ( | ) | const [inline] |
Get the variable bounds for this joint, in the same order as the names returned by getVariableNames()
Definition at line 150 of file joint_state.h.
unsigned int robot_state::JointState::getVariableCount | ( | ) | const [inline] |
Get the number of variable DOFs for this joint.
Definition at line 126 of file joint_state.h.
const std::map<std::string, unsigned int>& robot_state::JointState::getVariableIndexMap | ( | ) | const [inline] |
The set of variables that make up the state value of a joint are stored in some order. This map gives the position of each variable in that order, for each variable name.
Definition at line 175 of file joint_state.h.
const std::vector<std::string>& robot_state::JointState::getVariableNames | ( | ) | const [inline] |
Get the required name order for the joint state values.
Definition at line 144 of file joint_state.h.
const Eigen::Affine3d& robot_state::JointState::getVariableTransform | ( | ) | const [inline] |
Get the current variable transform.
Definition at line 156 of file joint_state.h.
Eigen::Affine3d& robot_state::JointState::getVariableTransform | ( | ) | [inline] |
Get the current variable transform.
Definition at line 162 of file joint_state.h.
const std::vector<double>& robot_state::JointState::getVariableValues | ( | ) | const [inline] |
Get the joint state values stored in the required order.
Definition at line 132 of file joint_state.h.
std::vector<double>& robot_state::JointState::getVariableValues | ( | ) | [inline] |
Get the joint state values stored in the required order.
Definition at line 138 of file joint_state.h.
std::vector<double>& robot_state::JointState::getVelocities | ( | ) | [inline] |
this will go away; don't use it
Definition at line 181 of file joint_state.h.
const std::vector<double>& robot_state::JointState::getVelocities | ( | ) | const [inline] |
this will go away; don't use it
Definition at line 193 of file joint_state.h.
void robot_state::JointState::interpolate | ( | const JointState * | to, |
const double | t, | ||
JointState * | dest | ||
) | const |
Definition at line 190 of file joint_state.cpp.
robot_state::JointState & robot_state::JointState::operator= | ( | const JointState & | other | ) |
Copy the data from one joint state to the other.
Definition at line 60 of file joint_state.cpp.
bool robot_state::JointState::satisfiesBounds | ( | double | margin = 0.0 | ) | const [inline] |
Checks if the current joint state values are all within the bounds set in the model.
Definition at line 98 of file joint_state.h.
bool robot_state::JointState::setVariableValue | ( | const std::string & | variable, |
double | value | ||
) |
Set the value of a particular variable for this joint.
Definition at line 72 of file joint_state.cpp.
void robot_state::JointState::setVariableValues | ( | const std::map< std::string, double > & | value_map | ) |
Sets the internal values from a map of joint variable names to actual values.
Definition at line 133 of file joint_state.cpp.
void robot_state::JointState::setVariableValues | ( | const std::map< std::string, double > & | value_map, |
std::vector< std::string > & | missing | ||
) |
Sets the internal values from a map of joint variable names to actual values. The function also fills the missing vector with the variable names that were not set.
Definition at line 110 of file joint_state.cpp.
bool robot_state::JointState::setVariableValues | ( | const std::vector< double > & | value_vector | ) |
Sets the internal values from the supplied vector, which are assumed to be in the required order.
Definition at line 89 of file joint_state.cpp.
void robot_state::JointState::setVariableValues | ( | const double * | value_vector | ) |
Sets the internal values from the supplied array, which are assumed to be in the required order. This function is intended to be fast, does no input checking and should be used carefully.
Definition at line 103 of file joint_state.cpp.
void robot_state::JointState::setVariableValues | ( | const Eigen::Affine3d & | transform | ) |
Sets the internal values from the transform.
Definition at line 165 of file joint_state.cpp.
Update the joints that mimic this one. This function is called automatically by the setVariable* functions.
Definition at line 172 of file joint_state.cpp.
friend class RobotState [friend] |
Definition at line 50 of file joint_state.h.
std::vector<double> robot_state::JointState::horrible_acceleration_placeholder_ [private] |
ignore this
Definition at line 219 of file joint_state.h.
std::vector<double> robot_state::JointState::horrible_velocity_placeholder_ [private] |
ignore this
Definition at line 216 of file joint_state.h.
const robot_model::JointModel* robot_state::JointState::joint_model_ [private] |
The joint model this state corresponds to.
Definition at line 207 of file joint_state.h.
std::vector<double> robot_state::JointState::joint_state_values_ [private] |
The joint values given in the order indicated by joint_variables_index_map_.
Definition at line 213 of file joint_state.h.
std::vector<JointState*> robot_state::JointState::mimic_requests_ [private] |
The set of joints that need to be updated when this one is.
Definition at line 222 of file joint_state.h.
Eigen::Affine3d robot_state::JointState::variable_transform_ [private] |
Tthe local transform (computed by forward kinematics)
Definition at line 210 of file joint_state.h.