Public Member Functions | Protected Attributes | List of all members
kinematic_constraints::PositionConstraint Class Reference

Class for constraints on the XYZ position of a link. More...

#include <kinematic_constraint.h>

Inheritance diagram for kinematic_constraints::PositionConstraint:
Inheritance graph
[legend]

Public Member Functions

virtual void clear ()
 Clear the stored constraint. More...
 
bool configure (const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf)
 Configure the constraint based on a moveit_msgs::PositionConstraint. More...
 
virtual ConstraintEvaluationResult decide (const robot_state::RobotState &state, bool verbose=false) const
 Decide whether the constraint is satisfied in the indicated state. More...
 
virtual bool enabled () const
 This function returns true if this constraint is configured and able to decide whether states do meet the constraint or not. If this function returns false it means that decide() will always return true – there is no constraint to be checked. More...
 
virtual bool equal (const KinematicConstraint &other, double margin) const
 Check if two constraints are the same. For position constraints this means that: More...
 
const std::vector< bodies::BodyPtr > & getConstraintRegions () const
 Returns all the constraint regions. More...
 
const robot_model::LinkModelgetLinkModel () const
 Returns the associated link model, or NULL if not enabled. More...
 
const Eigen::Vector3d & getLinkOffset () const
 Returns the target offset. More...
 
const std::string & getReferenceFrame () const
 Returns the reference frame. More...
 
bool hasLinkOffset () const
 If the constraint is enabled and the link offset is substantially different than zero. More...
 
bool mobileReferenceFrame () const
 If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false. More...
 
 PositionConstraint (const robot_model::RobotModelConstPtr &model)
 Constructor. More...
 
virtual void print (std::ostream &out=std::cout) const
 Print the constraint data. More...
 
- Public Member Functions inherited from kinematic_constraints::KinematicConstraint
double getConstraintWeight () const
 The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() function. More...
 
const robot_model::RobotModelConstPtr & getRobotModel () const
 
ConstraintType getType () const
 Get the type of constraint. More...
 
 KinematicConstraint (const robot_model::RobotModelConstPtr &model)
 Constructor. More...
 
virtual ~KinematicConstraint ()
 

Protected Attributes

std::string constraint_frame_id_
 The constraint frame id. More...
 
std::vector< bodies::BodyPtrconstraint_region_
 The constraint region vector. More...
 
EigenSTL::vector_Affine3d constraint_region_pose_
 The constraint region pose vector. More...
 
bool has_offset_
 Whether the offset is substantially different than 0.0. More...
 
const robot_model::LinkModellink_model_
 The link model constraint subject. More...
 
bool mobile_frame_
 Whether or not a mobile frame is employed. More...
 
Eigen::Vector3d offset_
 The target offset. More...
 
- Protected Attributes inherited from kinematic_constraints::KinematicConstraint
double constraint_weight_
 The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() function. More...
 
robot_model::RobotModelConstPtr robot_model_
 The kinematic model associated with this constraint. More...
 
ConstraintType type_
 The type of the constraint. More...
 

Additional Inherited Members

- Public Types inherited from kinematic_constraints::KinematicConstraint
enum  ConstraintType {
  UNKNOWN_CONSTRAINT, JOINT_CONSTRAINT, POSITION_CONSTRAINT, ORIENTATION_CONSTRAINT,
  VISIBILITY_CONSTRAINT
}
 Enum for representing a constraint. More...
 

Detailed Description

Class for constraints on the XYZ position of a link.

This class expresses X,Y,Z position constraints of a link. The position area is specified as a bounding volume consisting of one or more shapes - either solid primitives or meshes. The pose information in the volumes will be interpreted by using the header information. The header may either specify a fixed frame or a mobile frame. Additionally, a target offset specified in the frame of the link being constrained can be specified. The type value will return POSITION_CONSTRAINT.

Definition at line 505 of file kinematic_constraint.h.

Constructor & Destructor Documentation

kinematic_constraints::PositionConstraint::PositionConstraint ( const robot_model::RobotModelConstPtr &  model)
inline

Constructor.

Parameters
[in]modelThe kinematic model used for constraint evaluation

Definition at line 516 of file kinematic_constraint.h.

Member Function Documentation

void kinematic_constraints::PositionConstraint::clear ( )
virtual

Clear the stored constraint.

Implements kinematic_constraints::KinematicConstraint.

Definition at line 477 of file kinematic_constraint.cpp.

bool kinematic_constraints::PositionConstraint::configure ( const moveit_msgs::PositionConstraint &  pc,
const robot_state::Transforms tf 
)

Configure the constraint based on a moveit_msgs::PositionConstraint.

For the configure command to be successful, the link must be specified in the model, and one or more constrained regions must be correctly specified, which requires containing a valid shape and a pose for that shape. If the header frame on the constraint is empty, the constraint will fail to configure. If an invalid quaternion is passed for a shape, the identity quaternion will be substituted.

Parameters
[in]pcmoveit_msgs::PositionConstraint for configuration
Returns
True if constraint can be configured from pc

Definition at line 272 of file kinematic_constraint.cpp.

ConstraintEvaluationResult kinematic_constraints::PositionConstraint::decide ( const robot_state::RobotState state,
bool  verbose = false 
) const
virtual

Decide whether the constraint is satisfied in the indicated state.

Parameters
[in]stateThe kinematic state used for evaluation
[in]verboseWhether or not to print output
Returns

Implements kinematic_constraints::KinematicConstraint.

Definition at line 433 of file kinematic_constraint.cpp.

bool kinematic_constraints::PositionConstraint::enabled ( ) const
virtual

This function returns true if this constraint is configured and able to decide whether states do meet the constraint or not. If this function returns false it means that decide() will always return true – there is no constraint to be checked.

Implements kinematic_constraints::KinematicConstraint.

Definition at line 488 of file kinematic_constraint.cpp.

bool kinematic_constraints::PositionConstraint::equal ( const KinematicConstraint other,
double  margin 
) const
virtual

Check if two constraints are the same. For position constraints this means that:

  • The types are the same
  • The link model is the same
  • The frame of the constraints are the same
  • The target offsets are no more than the margin apart
  • Each entry in the constraint region of this constraint matches a region in the other constraint
  • Each entry in the other constraint region matches a region in the other constraint

Two constraint regions matching each other means that:

  • The poses match within the margin
  • The types are the same
  • The shape volumes are within the margin

Note that the two shapes can have different numbers of regions as long as all regions are matched up to another.

Parameters
[in]otherThe other constraint to test
[in]marginThe margin to apply to all values associated with constraint
Returns
True if equal, otherwise false

Implements kinematic_constraints::KinematicConstraint.

Definition at line 376 of file kinematic_constraint.cpp.

const std::vector<bodies::BodyPtr>& kinematic_constraints::PositionConstraint::getConstraintRegions ( ) const
inline

Returns all the constraint regions.

Returns
The constraint regions

Definition at line 611 of file kinematic_constraint.h.

const robot_model::LinkModel* kinematic_constraints::PositionConstraint::getLinkModel ( ) const
inline

Returns the associated link model, or NULL if not enabled.

Returns
The link model

Definition at line 575 of file kinematic_constraint.h.

const Eigen::Vector3d& kinematic_constraints::PositionConstraint::getLinkOffset ( ) const
inline

Returns the target offset.

Returns
The target offset

Definition at line 586 of file kinematic_constraint.h.

const std::string& kinematic_constraints::PositionConstraint::getReferenceFrame ( ) const
inline

Returns the reference frame.

Returns
The reference frame

Definition at line 622 of file kinematic_constraint.h.

bool kinematic_constraints::PositionConstraint::hasLinkOffset ( ) const
inline

If the constraint is enabled and the link offset is substantially different than zero.

Returns
Whether or not there is a link offset

Definition at line 598 of file kinematic_constraint.h.

bool kinematic_constraints::PositionConstraint::mobileReferenceFrame ( ) const
inline

If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false.

Returns
Whether a mobile reference frame is being employed

Definition at line 634 of file kinematic_constraint.h.

void kinematic_constraints::PositionConstraint::print ( std::ostream &  = std::cout) const
virtual

Print the constraint data.

Parameters
[in]outThe file descriptor for printing

Reimplemented from kinematic_constraints::KinematicConstraint.

Definition at line 469 of file kinematic_constraint.cpp.

Member Data Documentation

std::string kinematic_constraints::PositionConstraint::constraint_frame_id_
protected

The constraint frame id.

Definition at line 647 of file kinematic_constraint.h.

std::vector<bodies::BodyPtr> kinematic_constraints::PositionConstraint::constraint_region_
protected

The constraint region vector.

Definition at line 644 of file kinematic_constraint.h.

EigenSTL::vector_Affine3d kinematic_constraints::PositionConstraint::constraint_region_pose_
protected

The constraint region pose vector.

Definition at line 645 of file kinematic_constraint.h.

bool kinematic_constraints::PositionConstraint::has_offset_
protected

Whether the offset is substantially different than 0.0.

Definition at line 643 of file kinematic_constraint.h.

const robot_model::LinkModel* kinematic_constraints::PositionConstraint::link_model_
protected

The link model constraint subject.

Definition at line 648 of file kinematic_constraint.h.

bool kinematic_constraints::PositionConstraint::mobile_frame_
protected

Whether or not a mobile frame is employed.

Definition at line 646 of file kinematic_constraint.h.

Eigen::Vector3d kinematic_constraints::PositionConstraint::offset_
protected

The target offset.

Definition at line 642 of file kinematic_constraint.h.


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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:34