37 #ifndef MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_ 38 #define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_ 91 virtual bool configure(
const moveit_msgs::Constraints& constr);
118 bool configure(
const std::vector<kinematic_constraints::JointConstraint>& jc);
154 static const std::string SAMPLER_NAME =
"JointConstraintSampler";
169 min_bound_ = -std::numeric_limits<double>::max();
170 max_bound_ = std::numeric_limits<double>::max();
193 virtual void clear();
255 IKSamplingPose(
const kinematic_constraints::PositionConstraintPtr& pc);
264 IKSamplingPose(
const kinematic_constraints::OrientationConstraintPtr& oc);
274 IKSamplingPose(
const kinematic_constraints::PositionConstraintPtr& pc,
275 const kinematic_constraints::OrientationConstraintPtr& oc);
279 kinematic_constraints::OrientationConstraintPtr
335 virtual bool configure(
const moveit_msgs::Constraints& constr);
381 ik_timeout_ = timeout;
392 return sampling_pose_.position_constraint_;
402 return sampling_pose_.orientation_constraint_;
418 double getSamplingVolume()
const;
426 const std::string& getLinkName()
const;
450 unsigned int max_attempts);
476 unsigned int max_attempts);
485 static const std::string SAMPLER_NAME =
"IKConstraintSampler";
490 virtual void clear();
510 bool callIK(
const geometry_msgs::Pose& ik_query,
514 unsigned int max_attempts,
bool project);
519 kinematics::KinematicsBaseConstPtr
kb_;
double getIKTimeout() const
Gets the timeout argument passed to the IK solver.
std::vector< unsigned int > uindex_
The index of the unbounded joints in the joint state vector.
A structure for potentially holding a position constraint and an orientation constraint for use durin...
IKConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
Constructor.
Class for constraints on the orientation of a link.
void potentiallyAdjustMinMaxBounds(double min, double max)
Function that adjusts the joints only if they are more restrictive. This means that the min limit is ...
std::vector< double > values_
Values associated with this group to avoid continuously reallocating.
MOVEIT_CLASS_FORWARD(ConstraintSampler)
random_numbers::RandomNumberGenerator random_number_generator_
Random number generator used to sample.
ROSCPP_DECL bool validate(const std::string &name, std::string &error)
double ik_timeout_
Holds the timeout associated with IK.
const kinematic_constraints::OrientationConstraintPtr & getOrientationConstraint() const
Gets the orientation constraint associated with this sampler.
kinematic_constraints::OrientationConstraintPtr orientation_constraint_
Holds the orientation constraint for sampling.
JointConstraintSampler is a class that allows the sampling of joints in a particular group of the rob...
ConstraintSampler is an abstract base class that allows the sampling of a kinematic state for a parti...
std::size_t getUnconstrainedJointCount() const
Gets the number of unconstrained joints - joint that have no additional bound beyond the joint limits...
virtual void clear()
Clears all data from the constraint.
virtual const std::string & getName() const
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format...
std::size_t getConstrainedJointCount() const
Gets the number of constrained joints - joints that have an additional bound beyond the joint limits...
The constraint samplers namespace contains a number of methods for generating samples based on a cons...
std::string ik_frame_
Holds the base from of the IK solver.
virtual bool configure(const moveit_msgs::Constraints &constr)
Configures a joint constraint given a Constraints message.
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 generator used by the sampler.
const kinematic_constraints::PositionConstraintPtr & getPositionConstraint() const
Gets the position constraint associated with this sampler.
A class that allows the sampling of IK constraints.
void setIKTimeout(double timeout)
Sets the timeout argument passed to 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...
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
The signature for a callback that can compute IK.
Class for constraints on the XYZ position of a link.
Eigen::Affine3d eef_to_ik_tip_transform_
Holds the transformation from end effector to IK tip frame.
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 b...
An internal structure used for maintaining constraints on a particular joint.
bool transform_ik_
True if the frame associated with the kinematic model is different than the base frame of the IK solv...
virtual const std::string & getName() const
Get the name of the constraint sampler, for debugging purposes should be in CamelCase format...
IKSamplingPose sampling_pose_
Holder for the pose used for sampling.
std::vector< const robot_model::JointModel * > unbounded_
The joints that are not bounded except by joint limits.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
JointConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name)
kinematic_constraints::PositionConstraintPtr position_constraint_
Holds the position constraint for sampling.
kinematics::KinematicsBaseConstPtr kb_
Holds the kinematics solver.
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 para...