37 #ifndef MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_ 38 #define MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_ 47 #include <moveit_msgs/Constraints.h> 100 virtual void clear() = 0;
117 virtual bool enabled()
const = 0;
148 virtual void print(std::ostream& = std::cout)
const 161 return constraint_weight_;
214 type_ = JOINT_CONSTRAINT;
230 bool configure(
const moveit_msgs::JointConstraint& jc);
250 virtual bool enabled()
const;
251 virtual void clear();
252 virtual void print(std::ostream& out = std::cout)
const;
273 return local_variable_name_;
285 return joint_variable_name_;
290 return joint_variable_index_;
301 return joint_position_;
312 return joint_tolerance_above_;
323 return joint_tolerance_below_;
361 type_ = ORIENTATION_CONSTRAINT;
397 virtual void clear();
399 virtual bool enabled()
const;
400 virtual void print(std::ostream& out = std::cout)
const;
421 return desired_rotation_frame_id_;
432 return mobile_frame_;
442 return desired_rotation_matrix_;
453 return absolute_x_axis_tolerance_;
464 return absolute_y_axis_tolerance_;
475 return absolute_z_axis_tolerance_;
486 double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
518 type_ = POSITION_CONSTRAINT;
564 virtual void clear();
566 virtual bool enabled()
const;
567 virtual void print(std::ostream& out = std::cout)
const;
613 return constraint_region_;
624 return constraint_frame_id_;
638 return mobile_frame_;
795 virtual void clear();
819 virtual bool enabled()
const;
821 virtual void print(std::ostream& out = std::cout)
const;
903 bool add(
const std::vector<moveit_msgs::JointConstraint>& jc);
964 std::vector<ConstraintEvaluationResult>& results,
bool verbose =
false)
const;
988 void print(std::ostream& out = std::cout)
const;
998 return position_constraints_;
1009 return orientation_constraints_;
1020 return joint_constraints_;
1031 return visibility_constraints_;
1042 return all_constraints_;
1053 return kinematic_constraints_.empty();
1058 std::vector<KinematicConstraintPtr>
bool empty() const
Returns whether or not there are any constraints in the set.
ConstraintEvaluationResult(bool result_satisfied=false, double dist=0.0)
Constructor.
std::vector< moveit_msgs::JointConstraint > joint_constraints_
Messages corresponding to all internal joint constraints.
collision_detection::CollisionRobotPtr collision_robot_
A copy of the collision robot maintained for collision checking the cone against robot links...
const moveit_msgs::Constraints & getAllConstraints() const
Get all constraints in the set.
double absolute_z_axis_tolerance_
Storage for the tolerances.
Class for constraints on the orientation of a link.
virtual void print(std::ostream &=std::cout) const
Print the constraint data.
ConstraintType type_
The type of the constraint.
bool hasLinkOffset() const
If the constraint is enabled and the link offset is substantially different than zero.
int sensor_view_direction_
Storage for the sensor view direction.
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
Class for constraints on the visibility relationship between a sensor and a target.
int getJointVariableIndex() const
bool mobile_frame_
Whether or not a mobile frame is employed.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool mobile_sensor_frame_
True if the sensor is a non-fixed frame relative to the transform frame.
double getDesiredJointPosition() const
Gets the desired position component of the constraint.
EigenSTL::vector_Vector3d points_
A set of points along the base of the circle.
const robot_model::LinkModel * getLinkModel() const
Gets the subject link model.
MOVEIT_CLASS_FORWARD(KinematicConstraint)
const std::vector< bodies::BodyPtr > & getConstraintRegions() const
Returns all the constraint regions.
Class for handling single DOF joint constraints.
double max_view_angle_
Storage for the max view angle.
Eigen::Affine3d target_pose_
The target pose transformed into the transform frame.
double joint_tolerance_below_
Position and tolerance values.
A class that contains many different constraints, and can check RobotState *versus the full set...
const std::string & getReferenceFrame() const
Returns the reference frame.
const robot_model::LinkModel * getLinkModel() const
Returns the associated link model, or NULL if not enabled.
std::vector< moveit_msgs::PositionConstraint > position_constraints_
Messages corresponding to all internal position constraints.
const std::string & getJointVariableName() const
Gets the joint variable name, as known to the robot_model::RobotModel.
const std::vector< moveit_msgs::PositionConstraint > & getPositionConstraints() const
Get all position constraints in the set.
Representation and evaluation of kinematic constraints.
std::vector< moveit_msgs::VisibilityConstraint > visibility_constraints_
Messages corresponding to all internal visibility constraints.
robot_model::RobotModelConstPtr robot_model_
The kinematic model used for by the Set.
double distance
The distance evaluation from the constraint or constraints.
double target_radius_
Storage for the target radius.
const robot_model::LinkModel * link_model_
The target link model.
Eigen::Matrix3d desired_rotation_matrix_
The desired rotation matrix in the tf frame.
const Eigen::Vector3d & getLinkOffset() const
Returns the target offset.
double getJointToleranceBelow() const
Gets the lower tolerance component of the joint constraint.
const std::string & getLocalVariableName() const
Gets the local variable name if this constraint was configured for a part of a multi-DOF joint...
Eigen::Matrix3d desired_rotation_matrix_inv_
The inverse of the desired rotation matrix, precomputed for efficiency.
std::string sensor_frame_id_
The sensor frame id.
bool satisfied
Whether or not the constraint or constraints were satisfied.
double getZAxisTolerance() const
Gets the Z axis tolerance.
ConstraintType
Enum for representing a constraint.
int joint_variable_index_
The index of the joint variable name in the full robot state.
Eigen::Affine3d sensor_pose_
The sensor pose transformed into the transform frame.
std::string desired_rotation_frame_id_
The target frame of the transform tree.
std::string local_variable_name_
The local variable name for a multi DOF joint, if any.
std::vector< bodies::BodyPtr > constraint_region_
The constraint region vector.
const std::vector< moveit_msgs::JointConstraint > & getJointConstraints() const
Get all joint constraints in the set.
const std::string & getReferenceFrame() const
The target frame of the planning_models::Transforms class, for interpreting the rotation frame...
bool mobile_target_frame_
True if the target is a non-fixed frame relative to the transform frame.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
const robot_model::LinkModel * link_model_
The link model constraint subject.
robot_model::RobotModelConstPtr robot_model_
The kinematic model associated with this constraint.
std::string constraint_frame_id_
The constraint frame id.
Eigen::Vector3d offset_
The target offset.
double getConstraintWeight() const
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
Base class for representing a kinematic constraint.
bool has_offset_
Whether the offset is substantially different than 0.0.
moveit_msgs::Constraints all_constraints_
Messages corresponding to all internal constraints.
EigenSTL::vector_Affine3d constraint_region_pose_
The constraint region pose vector.
double getJointToleranceAbove() const
Gets the upper tolerance component of the joint constraint.
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Struct for containing the results of constraint evaluation.
Class for constraints on the XYZ position of a link.
const std::vector< moveit_msgs::VisibilityConstraint > & getVisibilityConstraints() const
Get all visibility constraints in the set.
std::string target_frame_id_
The target frame id.
unsigned int cone_sides_
Storage for the cone sides.
KinematicConstraintSet(const robot_model::RobotModelConstPtr &model)
Constructor.
std::string joint_variable_name_
The joint variable name.
double max_range_angle_
Storage for the max range angle.
const robot_model::JointModel * getJointModel() const
Get the joint model for which this constraint operates.
const robot_model::RobotModelConstPtr & getRobotModel() const
bool mobileReferenceFrame() const
Whether or not a mobile reference frame is being employed.
double getXAxisTolerance() const
Gets the X axis tolerance.
std::vector< KinematicConstraintPtr > kinematic_constraints_
Shared pointers to all the member constraints.
bool mobile_frame_
Whether or not the header frame is mobile or fixed.
OrientationConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
std::vector< moveit_msgs::OrientationConstraint > orientation_constraints_
Messages corresponding to all internal orientation constraints.
const robot_model::JointModel * joint_model_
The joint from the kinematic model for this constraint.
double getYAxisTolerance() const
Gets the Y axis tolerance.
Representation of a robot's state. This includes position, velocity, acceleration and effort...
A link from the robot. Contains the constant transform applied to the link and its geometry...
bool mobileReferenceFrame() const
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false...
double constraint_weight_
The weight of a constraint is a multiplicative factor associated to the distance computed by the deci...
const Eigen::Matrix3d & getDesiredRotationMatrix() const
The rotation target in the reference frame.
~KinematicConstraintSet()
JointConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
ConstraintType getType() const
Get the type of constraint.
PositionConstraint(const robot_model::RobotModelConstPtr &model)
Constructor.
void print(PropagationDistanceField &pdf, int numX, int numY, int numZ)
bool joint_is_continuous_
Whether or not the joint is continuous.
const std::vector< moveit_msgs::OrientationConstraint > & getOrientationConstraints() const
Get all orientation constraints in the set.