moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
A class that allows the sampling of IK constraints. More...
#include <default_constraint_samplers.h>
Public Member Functions | |
IKConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name) | |
Constructor. | |
IKConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, unsigned int seed) | |
Constructor. | |
bool | configure (const moveit_msgs::msg::Constraints &constr) override |
Configures the IK constraint given a constraints message. | |
bool | configure (const IKSamplingPose &sp) |
Configures the Constraint given a IKSamplingPose. | |
double | getIKTimeout () const |
Gets the timeout argument passed to the IK solver. | |
void | setIKTimeout (double timeout) |
Sets the timeout argument passed to the IK solver. | |
const kinematic_constraints::PositionConstraintPtr & | getPositionConstraint () const |
Gets the position constraint associated with this sampler. | |
const kinematic_constraints::OrientationConstraintPtr & | getOrientationConstraint () const |
Gets the orientation constraint associated with this sampler. | |
double | getSamplingVolume () const |
Gets the volume associated with the position and orientation constraints. | |
const std::string & | getLinkName () const |
Gets the link name associated with this sampler. | |
bool | sample (moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override |
Produces an IK sample. | |
bool | samplePose (Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const moveit::core::RobotState &ks, unsigned int max_attempts) |
Returns a pose that falls within the constraint regions. | |
const std::string & | getName () const override |
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format. | |
![]() | |
ConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name) | |
Constructor. | |
virtual | ~ConstraintSampler () |
const std::string & | getGroupName () const |
Gets the group name set in the constructor. | |
const moveit::core::JointModelGroup * | getJointModelGroup () const |
Gets the joint model group. | |
const planning_scene::PlanningSceneConstPtr & | getPlanningScene () const |
Gets the planning scene. | |
const std::vector< std::string > & | getFrameDependency () const |
Return the names of the mobile frames whose pose is needed when sample() is called. | |
const moveit::core::GroupStateValidityCallbackFn & | getGroupStateValidityCallback () const |
Gets the callback used to determine state validity during sampling. The sampler will attempt to satisfy this constraint if possible, but there is no guarantee. | |
void | setGroupStateValidityCallback (const moveit::core::GroupStateValidityCallbackFn &callback) |
Sets the callback used to determine the state validity during sampling. The sampler will attempt to satisfy this constraint if possible, but there is no guarantee. | |
bool | sample (moveit::core::RobotState &state) |
Samples given the constraints, populating state. The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in as the maximum number of attempts to make to take a sample. | |
bool | sample (moveit::core::RobotState &state, unsigned int max_attempts) |
Samples given the constraints, populating state. This function allows the parameter max_attempts to be set. | |
bool | sample (moveit::core::RobotState &state, const moveit::core::RobotState &reference_state) |
Samples given the constraints, populating state. The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in as the maximum number of attempts to make to take a sample. | |
bool | isValid () const |
Returns whether or not the constraint sampler is valid or not. To be valid, the joint model group must be available in the kinematic model and configure() must have successfully been called. | |
bool | getVerbose () const |
Check if the sampler is set to verbose mode. | |
virtual void | setVerbose (bool verbose) |
Enable/disable verbose mode for sampler. | |
Protected Member Functions | |
void | clear () override |
Clears all data from the constraint. | |
bool | loadIKSolver () |
Performs checks and sets various internal values associated with the IK solver. | |
bool | callIK (const geometry_msgs::msg::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout, moveit::core::RobotState &state, bool use_as_seed) |
Actually calls IK on the given pose, generating a random seed state. | |
bool | sampleHelper (moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) |
bool | validate (moveit::core::RobotState &state) const |
Protected Attributes | |
random_numbers::RandomNumberGenerator | random_number_generator_ |
Random generator used by the sampler. | |
IKSamplingPose | sampling_pose_ |
Holder for the pose used for sampling. | |
kinematics::KinematicsBaseConstPtr | kb_ |
Holds the kinematics solver. | |
double | ik_timeout_ |
Holds the timeout associated with IK. | |
std::string | ik_frame_ |
Holds the base from of the IK solver. | |
bool | transform_ik_ |
True if the frame associated with the kinematic model is different than the base frame of the IK solver. | |
bool | need_eef_to_ik_tip_transform_ |
True if the tip frame of the inverse kinematic is different than the frame of the end effector. | |
Eigen::Isometry3d | eef_to_ik_tip_transform_ |
Holds the transformation from end effector to IK tip frame. | |
![]() | |
bool | is_valid_ |
Holds the value for validity. | |
planning_scene::PlanningSceneConstPtr | scene_ |
Holds the planning scene. | |
const moveit::core::JointModelGroup *const | jmg_ |
Holds the joint model group associated with this constraint. | |
std::vector< std::string > | frame_depends_ |
Holds the set of frames that must exist in the reference state to allow samples to be drawn. | |
moveit::core::GroupStateValidityCallbackFn | group_state_validity_callback_ |
Holds the callback for state validity. | |
bool | verbose_ |
True if verbosity is on. | |
Additional Inherited Members | |
![]() | |
static const unsigned int | DEFAULT_MAX_SAMPLING_ATTEMPTS = 2 |
The default value associated with a sampling request. By default if a valid sample cannot be produced in this many attempts, it returns with no sample. | |
A class that allows the sampling of IK constraints.
An IK constraint can have a position constraint, and orientation constraint, or both. The constraint will attempt to sample a pose that adheres to the constraint, and then solves IK for that pose.
Definition at line 312 of file default_constraint_samplers.h.
|
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 325 of file default_constraint_samplers.h.
|
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 |
[in] | seed | The rng seed to be used |
Definition at line 342 of file default_constraint_samplers.h.
|
protected |
Actually calls IK on the given pose, generating a random seed state.
ik_query | The pose for solving IK, assumed to be for the tip frame in the base frame |
timeout | The timeout for the IK search |
jsg | The joint state group into which to place the solution |
use_as_seed | If true, the state values in jsg are used as seed for the IK |
Definition at line 663 of file default_constraint_samplers.cpp.
|
overrideprotectedvirtual |
Clears all data from the constraint.
Reimplemented from constraint_samplers::ConstraintSampler.
Definition at line 243 of file default_constraint_samplers.cpp.
bool constraint_samplers::IKConstraintSampler::configure | ( | const IKSamplingPose & | sp | ) |
Configures the Constraint given a IKSamplingPose.
This function performs the actual constraint configuration. It returns true if the following are true:
[in] | sp | The variable that contains the position and orientation constraints |
Definition at line 253 of file default_constraint_samplers.cpp.
|
overridevirtual |
Configures the IK constraint given a constraints message.
If the constraints message contains both orientation constraints and positions constraints, the function iterates through each potential pair until it finds a pair of position orientation constraints that lead to valid configuration of kinematic constraints. It creates an IKSamplingPose from these and calls configure(const IKSamplingPose &sp). If no pair leads to both having valid configuration, it will attempt to iterate through the position constraints in the Constraints message, calling configure(const IKSamplingPose &sp) on the resulting IKSamplingPose. Finally, if no valid position constraints exist it will attempt the same procedure with the orientation constraints. If no valid position or orientation constraints exist, it will return false. For more information, see the docs for configure(const IKSamplingPose &sp).
constr | The Constraint message |
Implements constraint_samplers::ConstraintSampler.
Definition at line 295 of file default_constraint_samplers.cpp.
|
inline |
Gets the timeout argument passed to the IK solver.
Definition at line 406 of file default_constraint_samplers.h.
const std::string & constraint_samplers::IKConstraintSampler::getLinkName | ( | ) | const |
Gets the link name associated with this sampler.
Definition at line 355 of file default_constraint_samplers.cpp.
|
inlineoverridevirtual |
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format.
Implements constraint_samplers::ConstraintSampler.
Definition at line 519 of file default_constraint_samplers.h.
|
inline |
Gets the orientation constraint associated with this sampler.
Definition at line 437 of file default_constraint_samplers.h.
|
inline |
Gets the position constraint associated with this sampler.
Definition at line 427 of file default_constraint_samplers.h.
double constraint_samplers::IKConstraintSampler::getSamplingVolume | ( | ) | const |
Gets the volume associated with the position and orientation constraints.
This function computes the volume of the sampling constraint. The volume associated with the position constraint is either the product of the volume of all position constraint regions, or 1.0 otherwise. The volume associated with the orientation constraint is the product of all the axis tolerances, or 1.0 otherwise. If both are specified, the product of the volumes is returned.
Definition at line 332 of file default_constraint_samplers.cpp.
|
protected |
Performs checks and sets various internal values associated with the IK solver.
Definition at line 362 of file default_constraint_samplers.cpp.
|
overridevirtual |
Produces an IK sample.
This function first calls the samplePose function to produce a position and orientation in the constraint region. It then calls IK on that pose. If a pose that satisfies the constraints can be determined, and IK returns a solution for that pose, then the joint values associated with the joint group will be populated with the results of the IK, and the function will return true. The function will attempt to sample a pose up to max_attempts number of times, and then call IK on that value. If IK is not successful, it will repeat the pose sample and IK procedure max_attempt times. If in any iteration a valid pose cannot be sample within max_attempts time, it will return false.
jsg | The joint state group in question. Must match the group passed in the constructor or will return false. |
ks | A reference state that will be used for transforming the IK poses |
max_attempts | The number of attempts to both sample and try IK |
Implements constraint_samplers::ConstraintSampler.
Definition at line 581 of file default_constraint_samplers.cpp.
|
protected |
Definition at line 587 of file default_constraint_samplers.cpp.
bool constraint_samplers::IKConstraintSampler::samplePose | ( | Eigen::Vector3d & | pos, |
Eigen::Quaterniond & | quat, | ||
const moveit::core::RobotState & | ks, | ||
unsigned int | max_attempts | ||
) |
Returns a pose that falls within the constraint regions.
If a position constraint is specified, then a position is produced by selecting a random region among the constraint regions and taking a sample in that region. If no region is valid the function returns false. If no position constraint is specified, a position is produced by assigning a random valid value to each group joint, performing forward kinematics, and taking the resulting pose. If an orientation constraint is specified, then an quaternion is produced by sampling a difference value within the axis tolerances and applying the difference rotation to the orientation constraint target. Otherwise, a random quaternion is produced.
[out] | pos | The position component of the sample |
[out] | quat | The orientation component of the sample. It will always be a normalized quaternion. |
[in] | ks | The reference state used for performing transforms |
[in] | max_attempts | The maximum attempts to try to sample - applies only to the position constraint |
Definition at line 441 of file default_constraint_samplers.cpp.
|
inline |
Sets the timeout argument passed to the IK solver.
timeout | The timeout argument that will be used in future IK calls |
Definition at line 416 of file default_constraint_samplers.h.
|
protected |
Definition at line 654 of file default_constraint_samplers.cpp.
|
protected |
Holds the transformation from end effector to IK tip frame.
Definition at line 562 of file default_constraint_samplers.h.
|
protected |
Holds the base from of the IK solver.
Definition at line 557 of file default_constraint_samplers.h.
|
protected |
Holds the timeout associated with IK.
Definition at line 556 of file default_constraint_samplers.h.
|
protected |
Holds the kinematics solver.
Definition at line 555 of file default_constraint_samplers.h.
|
protected |
True if the tip frame of the inverse kinematic is different than the frame of the end effector.
Definition at line 560 of file default_constraint_samplers.h.
|
protected |
Random generator used by the sampler.
Definition at line 553 of file default_constraint_samplers.h.
|
protected |
Holder for the pose used for sampling.
Definition at line 554 of file default_constraint_samplers.h.
|
protected |
True if the frame associated with the kinematic model is different than the base frame of the IK solver.
Definition at line 558 of file default_constraint_samplers.h.