moveit2
The MoveIt Motion Planning Framework for ROS 2.
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
constraint_samplers::IKConstraintSampler Class Reference

A class that allows the sampling of IK constraints. More...

#include <default_constraint_samplers.h>

Inheritance diagram for constraint_samplers::IKConstraintSampler:
Inheritance graph
[legend]
Collaboration diagram for constraint_samplers::IKConstraintSampler:
Collaboration graph
[legend]

Public Member Functions

 IKConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
 Constructor. More...
 
 IKConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, unsigned int seed)
 Constructor. More...
 
bool configure (const moveit_msgs::msg::Constraints &constr) override
 Configures the IK constraint given a constraints message. More...
 
bool configure (const IKSamplingPose &sp)
 Configures the Constraint given a IKSamplingPose. More...
 
double getIKTimeout () const
 Gets the timeout argument passed to the IK solver. More...
 
void setIKTimeout (double timeout)
 Sets the timeout argument passed to the IK solver. More...
 
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint () const
 Gets the position constraint associated with this sampler. More...
 
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint () const
 Gets the orientation constraint associated with this sampler. More...
 
double getSamplingVolume () const
 Gets the volume associated with the position and orientation constraints. More...
 
const std::string & getLinkName () const
 Gets the link name associated with this sampler. More...
 
bool sample (moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts) override
 Produces an IK sample. More...
 
bool project (moveit::core::RobotState &state, unsigned int max_attempts) override
 Project a sample given the constraints, updating the joint state group. This function allows the parameter max_attempts to be set. More...
 
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. More...
 
const std::string & getName () const override
 Get the name of the constraint sampler, for debugging purposes should be in CamelCase format. More...
 
- Public Member Functions inherited from constraint_samplers::ConstraintSampler
 ConstraintSampler (const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
 Constructor. More...
 
virtual ~ConstraintSampler ()
 
const std::string & getGroupName () const
 Gets the group name set in the constructor. More...
 
const moveit::core::JointModelGroupgetJointModelGroup () const
 Gets the joint model group. More...
 
const planning_scene::PlanningSceneConstPtr & getPlanningScene () const
 Gets the planning scene. More...
 
const std::vector< std::string > & getFrameDependency () const
 Return the names of the mobile frames whose pose is needed when sample() is called. More...
 
const moveit::core::GroupStateValidityCallbackFngetGroupStateValidityCallback () 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. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
bool project (moveit::core::RobotState &state)
 Project a sample given the constraints, updating the joint state group. The value DEFAULT_MAX_SAMPLING_ATTEMPTS will be passed in as the maximum number of attempts to make to project the sample. More...
 
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. More...
 
bool getVerbose () const
 Check if the sampler is set to verbose mode. More...
 
virtual void setVerbose (bool verbose)
 Enable/disable verbose mode for sampler. More...
 

Protected Member Functions

void clear () override
 Clears all data from the constraint. More...
 
bool loadIKSolver ()
 Performs checks and sets various internal values associated with the IK solver. More...
 
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. More...
 
bool sampleHelper (moveit::core::RobotState &state, const moveit::core::RobotState &reference_state, unsigned int max_attempts, bool project)
 
bool validate (moveit::core::RobotState &state) const
 

Protected Attributes

random_numbers::RandomNumberGenerator random_number_generator_
 Random generator used by the sampler. More...
 
IKSamplingPose sampling_pose_
 Holder for the pose used for sampling. More...
 
kinematics::KinematicsBaseConstPtr kb_
 Holds the kinematics solver. More...
 
double ik_timeout_
 Holds the timeout associated with IK. More...
 
std::string ik_frame_
 Holds the base from of the IK solver. More...
 
bool transform_ik_
 True if the frame associated with the kinematic model is different than the base frame of the IK solver. More...
 
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. More...
 
Eigen::Isometry3d eef_to_ik_tip_transform_
 Holds the transformation from end effector to IK tip frame. More...
 
- Protected Attributes inherited from constraint_samplers::ConstraintSampler
bool is_valid_
 Holds the value for validity. More...
 
planning_scene::PlanningSceneConstPtr scene_
 Holds the planning scene. More...
 
const moveit::core::JointModelGroup *const jmg_
 Holds the joint model group associated with this constraint. More...
 
std::vector< std::string > frame_depends_
 Holds the set of frames that must exist in the reference state to allow samples to be drawn. More...
 
moveit::core::GroupStateValidityCallbackFn group_state_validity_callback_
 Holds the callback for state validity. More...
 
bool verbose_
 True if verbosity is on. More...
 

Additional Inherited Members

- Static Public Attributes inherited from constraint_samplers::ConstraintSampler
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. More...
 

Detailed Description

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 314 of file default_constraint_samplers.h.

Constructor & Destructor Documentation

◆ IKConstraintSampler() [1/2]

constraint_samplers::IKConstraintSampler::IKConstraintSampler ( const planning_scene::PlanningSceneConstPtr &  scene,
const std::string &  group_name 
)
inline

Constructor.

Parameters
[in]sceneThe planning scene used to check the constraint
[in]group_nameThe 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 327 of file default_constraint_samplers.h.

◆ IKConstraintSampler() [2/2]

constraint_samplers::IKConstraintSampler::IKConstraintSampler ( const planning_scene::PlanningSceneConstPtr &  scene,
const std::string &  group_name,
unsigned int  seed 
)
inline

Constructor.

Parameters
[in]sceneThe planning scene used to check the constraint
[in]group_nameThe 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]seedThe rng seed to be used

Definition at line 344 of file default_constraint_samplers.h.

Member Function Documentation

◆ callIK()

bool constraint_samplers::IKConstraintSampler::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 
)
protected

Actually calls IK on the given pose, generating a random seed state.

Parameters
ik_queryThe pose for solving IK, assumed to be for the tip frame in the base frame
timeoutThe timeout for the IK search
jsgThe joint state group into which to place the solution
use_as_seedIf true, the state values in jsg are used as seed for the IK
Returns
True if IK returns successfully with the timeout, and otherwise false.

Definition at line 634 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ clear()

void constraint_samplers::IKConstraintSampler::clear ( )
overrideprotectedvirtual

Clears all data from the constraint.

Reimplemented from constraint_samplers::ConstraintSampler.

Definition at line 233 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ configure() [1/2]

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:

  • The IKSamplingPose has either a valid orientation or position constraint
  • The position and orientation constraints are specified for the same link
  • There is a valid IK solver instance for the indicated group. This will be only be the case if a group has a specific solver associated with it. For situations where the super-group doesn't have a solver, but all subgroups have solvers, then use the ConstraintSamplerManager.
  • The kinematic model has both the links associated with the IK solver's tip and base frames.
  • The link specified in the constraints is the tip link of the IK solver
Parameters
[in]spThe variable that contains the position and orientation constraints
Returns
True if all conditions are met and the group specified in the constructor is valid. Otherwise, false.

Definition at line 243 of file default_constraint_samplers.cpp.

Here is the call graph for this function:

◆ configure() [2/2]

bool constraint_samplers::IKConstraintSampler::configure ( const moveit_msgs::msg::Constraints &  constr)
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).

Parameters
constrThe Constraint message
Returns
True if some valid position and orientation constraints exist and the overloaded configuration function returns true. Otherwise, returns false.

Implements constraint_samplers::ConstraintSampler.

Definition at line 283 of file default_constraint_samplers.cpp.

Here is the caller graph for this function:

◆ getIKTimeout()

double constraint_samplers::IKConstraintSampler::getIKTimeout ( ) const
inline

Gets the timeout argument passed to the IK solver.

Returns
The IK timeout

Definition at line 408 of file default_constraint_samplers.h.

◆ getLinkName()

const std::string & constraint_samplers::IKConstraintSampler::getLinkName ( ) const

Gets the link name associated with this sampler.

Returns
The associated link name

Definition at line 337 of file default_constraint_samplers.cpp.

◆ getName()

const std::string& constraint_samplers::IKConstraintSampler::getName ( ) const
inlineoverridevirtual

Get the name of the constraint sampler, for debugging purposes should be in CamelCase format.

Returns
string of name

Implements constraint_samplers::ConstraintSampler.

Definition at line 522 of file default_constraint_samplers.h.

◆ getOrientationConstraint()

const kinematic_constraints::OrientationConstraintPtr& constraint_samplers::IKConstraintSampler::getOrientationConstraint ( ) const
inline

Gets the orientation constraint associated with this sampler.

Returns
The orientation constraint, or an empty shared_ptr if none has been specified

Definition at line 439 of file default_constraint_samplers.h.

Here is the caller graph for this function:

◆ getPositionConstraint()

const kinematic_constraints::PositionConstraintPtr& constraint_samplers::IKConstraintSampler::getPositionConstraint ( ) const
inline

Gets the position constraint associated with this sampler.

Returns
The position constraint, or an empty shared_ptr if none has been specified

Definition at line 429 of file default_constraint_samplers.h.

Here is the caller graph for this function:

◆ getSamplingVolume()

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.

Returns
Returns the sum of the volumes of all constraint regions associated with the position and orientation constraints.

Definition at line 316 of file default_constraint_samplers.cpp.

◆ loadIKSolver()

bool constraint_samplers::IKConstraintSampler::loadIKSolver ( )
protected

Performs checks and sets various internal values associated with the IK solver.

Returns
True if the IK solver exists and if it associated with the expected base frame and tip frames. Otherwise false.

Definition at line 344 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ project()

bool constraint_samplers::IKConstraintSampler::project ( moveit::core::RobotState state,
unsigned int  max_attempts 
)
overridevirtual

Project a sample given the constraints, updating the joint state group. This function allows the parameter max_attempts to be set.

Parameters
[out]stateThe state into which the values will be placed. Only values for the group are written.
[in]max_attemptsThe 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.
Returns
True if a sample was successfully projected, false otherwise

Implements constraint_samplers::ConstraintSampler.

Definition at line 620 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ sample()

bool constraint_samplers::IKConstraintSampler::sample ( moveit::core::RobotState state,
const moveit::core::RobotState reference_state,
unsigned int  max_attempts 
)
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.

Parameters
jsgThe joint state group in question. Must match the group passed in the constructor or will return false.
ksA reference state that will be used for transforming the IK poses
max_attemptsThe number of attempts to both sample and try IK
Returns
True if a valid sample pose was produced and valid IK found for that pose. Otherwise false.

Implements constraint_samplers::ConstraintSampler.

Definition at line 549 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ sampleHelper()

bool constraint_samplers::IKConstraintSampler::sampleHelper ( moveit::core::RobotState state,
const moveit::core::RobotState reference_state,
unsigned int  max_attempts,
bool  project 
)
protected

Definition at line 555 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ samplePose()

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.

Parameters
[out]posThe position component of the sample
[out]quatThe orientation component of the sample. It will always be a normalized quaternion.
[in]ksThe reference state used for performing transforms
[in]max_attemptsThe maximum attempts to try to sample - applies only to the position constraint
Returns
True if a sample was successfully produced, otherwise false

Definition at line 417 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ setIKTimeout()

void constraint_samplers::IKConstraintSampler::setIKTimeout ( double  timeout)
inline

Sets the timeout argument passed to the IK solver.

Parameters
timeoutThe timeout argument that will be used in future IK calls

Definition at line 418 of file default_constraint_samplers.h.

◆ validate()

bool constraint_samplers::IKConstraintSampler::validate ( moveit::core::RobotState state) const
protected

Definition at line 625 of file default_constraint_samplers.cpp.

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ eef_to_ik_tip_transform_

Eigen::Isometry3d constraint_samplers::IKConstraintSampler::eef_to_ik_tip_transform_
protected

Holds the transformation from end effector to IK tip frame.

Definition at line 565 of file default_constraint_samplers.h.

◆ ik_frame_

std::string constraint_samplers::IKConstraintSampler::ik_frame_
protected

Holds the base from of the IK solver.

Definition at line 560 of file default_constraint_samplers.h.

◆ ik_timeout_

double constraint_samplers::IKConstraintSampler::ik_timeout_
protected

Holds the timeout associated with IK.

Definition at line 559 of file default_constraint_samplers.h.

◆ kb_

kinematics::KinematicsBaseConstPtr constraint_samplers::IKConstraintSampler::kb_
protected

Holds the kinematics solver.

Definition at line 558 of file default_constraint_samplers.h.

◆ need_eef_to_ik_tip_transform_

bool constraint_samplers::IKConstraintSampler::need_eef_to_ik_tip_transform_
protected

True if the tip frame of the inverse kinematic is different than the frame of the end effector.

Definition at line 563 of file default_constraint_samplers.h.

◆ random_number_generator_

random_numbers::RandomNumberGenerator constraint_samplers::IKConstraintSampler::random_number_generator_
protected

Random generator used by the sampler.

Definition at line 556 of file default_constraint_samplers.h.

◆ sampling_pose_

IKSamplingPose constraint_samplers::IKConstraintSampler::sampling_pose_
protected

Holder for the pose used for sampling.

Definition at line 557 of file default_constraint_samplers.h.

◆ transform_ik_

bool constraint_samplers::IKConstraintSampler::transform_ik_
protected

True if the frame associated with the kinematic model is different than the base frame of the IK solver.

Definition at line 561 of file default_constraint_samplers.h.


The documentation for this class was generated from the following files: