JointConstraintSampler is a class that allows the sampling of joints in a particular group of the robot, subject to a set of individual joint constraints. More...
#include <default_constraint_samplers.h>
Classes | |
struct | JointInfo |
An internal structure used for maintaining constraints on a particular joint. More... | |
Public Member Functions | |
virtual bool | configure (const moveit_msgs::Constraints &constr) |
Configures a joint constraint given a Constraints message. | |
bool | configure (const std::vector< kinematic_constraints::JointConstraint > &jc) |
Configures a joint constraint given a vector of constraints. | |
std::size_t | getConstrainedJointCount () const |
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits. | |
virtual const std::string & | getName () const |
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format. | |
std::size_t | getUnconstrainedJointCount () const |
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits. | |
JointConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name) | |
virtual bool | project (robot_state::RobotState &state, unsigned int max_attempts) |
Project a sample given the constraints, updating the joint state group. This function allows the parameter max_attempts to be set. | |
virtual bool | sample (robot_state::RobotState &state, const robot_state::RobotState &ks, unsigned int max_attempts) |
Samples given the constraints, populating state. This function allows the parameter max_attempts to be set. | |
Protected Member Functions | |
virtual void | clear () |
Clears all data from the constraint. | |
Protected Attributes | |
std::vector< JointInfo > | bounds_ |
The bounds for any joint with bounds that are more restrictive than the joint limits. | |
random_numbers::RandomNumberGenerator | random_number_generator_ |
Random number generator used to sample. | |
std::vector< unsigned int > | uindex_ |
The index of the unbounded joints in the joint state vector. | |
std::vector< const robot_model::JointModel * > | unbounded_ |
The joints that are not bounded except by joint limits. | |
std::vector< double > | values_ |
Values associated with this group to avoid continuously reallocating. |
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the robot, subject to a set of individual joint constraints.
The set of individual joint constraint reduce the allowable bounds used in the sampling. Unconstrained values will be sampled within their limits.
Definition at line 55 of file default_constraint_samplers.h.
constraint_samplers::JointConstraintSampler::JointConstraintSampler | ( | const planning_scene::PlanningSceneConstPtr & | scene, |
const std::string & | group_name | ||
) | [inline] |
Constructor
[in] | scene | The planning scene used to check the constraint |
[in] | group_name | The group name associated with the constraint. Will be invalid if no group name is passed in or the joint model group cannot be found in the kinematic model |
Definition at line 69 of file default_constraint_samplers.h.
void constraint_samplers::JointConstraintSampler::clear | ( | ) | [protected, virtual] |
Clears all data from the constraint.
Reimplemented from constraint_samplers::ConstraintSampler.
Definition at line 179 of file default_constraint_samplers.cpp.
bool constraint_samplers::JointConstraintSampler::configure | ( | const moveit_msgs::Constraints & | constr | ) | [virtual] |
Configures a joint constraint given a Constraints message.
If more than one constraint for a particular joint is specified, the most restrictive set of bounds will be used (highest minimum value, lowest maximum value). For the configuration to be successful, the following condition must be met, in addition to the conditions specified in configure(const std::vector<kinematic_constraints::JointConstraint> &jc) :
[in] | constr | The message containing the constraints |
Implements constraint_samplers::ConstraintSampler.
Definition at line 43 of file default_constraint_samplers.cpp.
bool constraint_samplers::JointConstraintSampler::configure | ( | const std::vector< kinematic_constraints::JointConstraint > & | jc | ) |
Configures a joint constraint given a vector of constraints.
If more than one constraint for a particular joint is specified, the most restrictive set of bounds will be used (highest minimum value, lowest_maximum value. For the configuration to be successful, the following conditions must be met:
[in] | jc | The vector of joint constraints |
Definition at line 57 of file default_constraint_samplers.cpp.
std::size_t constraint_samplers::JointConstraintSampler::getConstrainedJointCount | ( | ) | const [inline] |
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits.
Definition at line 134 of file default_constraint_samplers.h.
virtual const std::string& constraint_samplers::JointConstraintSampler::getName | ( | ) | const [inline, virtual] |
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format.
Implements constraint_samplers::ConstraintSampler.
Definition at line 155 of file default_constraint_samplers.h.
std::size_t constraint_samplers::JointConstraintSampler::getUnconstrainedJointCount | ( | ) | const [inline] |
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits.
Definition at line 145 of file default_constraint_samplers.h.
bool constraint_samplers::JointConstraintSampler::project | ( | robot_state::RobotState & | state, |
unsigned int | max_attempts | ||
) | [virtual] |
Project a sample given the constraints, updating the joint state group. This function allows the parameter max_attempts to be set.
[out] | state | The state into which the values will be placed. Only values for the group are written. |
[in] | max_attempts | The maximum number of times to attempt to draw a sample. If no sample has been drawn in this number of attempts, false will be returned. |
Implements constraint_samplers::ConstraintSampler.
Definition at line 173 of file default_constraint_samplers.cpp.
bool constraint_samplers::JointConstraintSampler::sample | ( | robot_state::RobotState & | state, |
const robot_state::RobotState & | reference_state, | ||
unsigned int | max_attempts | ||
) | [virtual] |
Samples given the constraints, populating state. This function allows the parameter max_attempts to be set.
[out] | state | The state into which the values will be placed. Only values for the group are written. |
[in] | reference_state | Reference state that will be used to do transforms or perform other actions |
[in] | max_attempts | The maximum number of times to attempt to draw a sample. If no sample has been drawn in this number of attempts, false will be returned. |
Implements constraint_samplers::ConstraintSampler.
Definition at line 143 of file default_constraint_samplers.cpp.
std::vector<JointInfo> constraint_samplers::JointConstraintSampler::bounds_ [protected] |
The bounds for any joint with bounds that are more restrictive than the joint limits.
Definition at line 200 of file default_constraint_samplers.h.
random_numbers::RandomNumberGenerator constraint_samplers::JointConstraintSampler::random_number_generator_ [protected] |
Random number generator used to sample.
Definition at line 199 of file default_constraint_samplers.h.
std::vector<unsigned int> constraint_samplers::JointConstraintSampler::uindex_ [protected] |
The index of the unbounded joints in the joint state vector.
Definition at line 203 of file default_constraint_samplers.h.
std::vector<const robot_model::JointModel*> constraint_samplers::JointConstraintSampler::unbounded_ [protected] |
The joints that are not bounded except by joint limits.
Definition at line 202 of file default_constraint_samplers.h.
std::vector<double> constraint_samplers::JointConstraintSampler::values_ [protected] |
Values associated with this group to avoid continuously reallocating.
Definition at line 204 of file default_constraint_samplers.h.