Class for constraints on the XYZ position of a link. More...
#include <kinematic_constraint.h>
Public Member Functions | |
virtual void | clear () |
Clear the stored constraint. | |
bool | configure (const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf) |
Configure the constraint based on a moveit_msgs::PositionConstraint. | |
virtual ConstraintEvaluationResult | decide (const robot_state::RobotState &state, bool verbose=false) const |
Decide whether the constraint is satisfied in the indicated state. | |
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. | |
virtual bool | equal (const KinematicConstraint &other, double margin) const |
Check if two constraints are the same. For position constraints this means that: | |
const std::vector < bodies::BodyPtr > & | getConstraintRegions () const |
Returns all the constraint regions. | |
const robot_model::LinkModel * | getLinkModel () const |
Returns the associated link model, or NULL if not enabled. | |
const Eigen::Vector3d & | getLinkOffset () const |
Returns the target offset. | |
const std::string & | getReferenceFrame () const |
Returns the reference frame. | |
bool | hasLinkOffset () const |
If the constraint is enabled and the link offset is substantially different than zero. | |
bool | mobileReferenceFrame () const |
If enabled and the specified frame is a mobile frame, return true. Otherwise, returns false. | |
PositionConstraint (const robot_model::RobotModelConstPtr &model) | |
Constructor. | |
virtual void | print (std::ostream &out=std::cout) const |
Print the constraint data. | |
void | swapLinkModel (const robot_model::LinkModel *new_link, const Eigen::Affine3d &update) |
Change this constraint to a different link, applying a specified transform to the constraint region. This should be used for links that are associated to each other via fixed transforms. | |
Protected Attributes | |
std::string | constraint_frame_id_ |
The constraint frame id. | |
std::vector< bodies::BodyPtr > | constraint_region_ |
The constraint region vector. | |
EigenSTL::vector_Affine3d | constraint_region_pose_ |
The constraint region pose vector. | |
bool | has_offset_ |
Whether the offset is substantially different than 0.0. | |
const robot_model::LinkModel * | link_model_ |
The link model constraint subject. | |
bool | mobile_frame_ |
Whether or not a mobile frame is employed. | |
Eigen::Vector3d | offset_ |
The target offset. |
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 501 of file kinematic_constraint.h.
kinematic_constraints::PositionConstraint::PositionConstraint | ( | const robot_model::RobotModelConstPtr & | model | ) | [inline] |
Constructor.
[in] | model | The kinematic model used for constraint evaluation |
Definition at line 513 of file kinematic_constraint.h.
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.
[in] | pc | moveit_msgs::PositionConstraint for configuration |
Definition at line 270 of file kinematic_constraint.cpp.
kinematic_constraints::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.
[in] | state | The kinematic state used for evaluation |
[in] | verbose | Whether or not to print output |
Implements kinematic_constraints::KinematicConstraint.
Definition at line 437 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:
Two constraint regions matching each other means that:
Note that the two shapes can have different numbers of regions as long as all regions are matched up to another.
[in] | other | The other constraint to test |
[in] | margin | The margin to apply to all values associated with constraint |
Implements kinematic_constraints::KinematicConstraint.
Definition at line 378 of file kinematic_constraint.cpp.
const std::vector<bodies::BodyPtr>& kinematic_constraints::PositionConstraint::getConstraintRegions | ( | ) | const [inline] |
Returns all the constraint regions.
Definition at line 609 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.
Definition at line 573 of file kinematic_constraint.h.
const Eigen::Vector3d& kinematic_constraints::PositionConstraint::getLinkOffset | ( | ) | const [inline] |
Returns the target offset.
Definition at line 584 of file kinematic_constraint.h.
const std::string& kinematic_constraints::PositionConstraint::getReferenceFrame | ( | ) | const [inline] |
Returns the reference frame.
Definition at line 620 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.
Definition at line 596 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.
Definition at line 632 of file kinematic_constraint.h.
void kinematic_constraints::PositionConstraint::print | ( | std::ostream & | out = std::cout | ) | const [virtual] |
Print the constraint data.
[in] | out | The file descriptor for printing |
Reimplemented from kinematic_constraints::KinematicConstraint.
Definition at line 469 of file kinematic_constraint.cpp.
void kinematic_constraints::PositionConstraint::swapLinkModel | ( | const robot_model::LinkModel * | new_link, |
const Eigen::Affine3d & | update | ||
) |
Change this constraint to a different link, applying a specified transform to the constraint region. This should be used for links that are associated to each other via fixed transforms.
Definition at line 369 of file kinematic_constraint.cpp.
std::string kinematic_constraints::PositionConstraint::constraint_frame_id_ [protected] |
The constraint frame id.
Definition at line 649 of file kinematic_constraint.h.
std::vector<bodies::BodyPtr> kinematic_constraints::PositionConstraint::constraint_region_ [protected] |
The constraint region vector.
Definition at line 646 of file kinematic_constraint.h.
EigenSTL::vector_Affine3d kinematic_constraints::PositionConstraint::constraint_region_pose_ [protected] |
The constraint region pose vector.
Definition at line 647 of file kinematic_constraint.h.
bool kinematic_constraints::PositionConstraint::has_offset_ [protected] |
Whether the offset is substantially different than 0.0.
Definition at line 645 of file kinematic_constraint.h.
const robot_model::LinkModel* kinematic_constraints::PositionConstraint::link_model_ [protected] |
The link model constraint subject.
Definition at line 650 of file kinematic_constraint.h.
bool kinematic_constraints::PositionConstraint::mobile_frame_ [protected] |
Whether or not a mobile frame is employed.
Definition at line 648 of file kinematic_constraint.h.
Eigen::Vector3d kinematic_constraints::PositionConstraint::offset_ [protected] |
The target offset.
Definition at line 644 of file kinematic_constraint.h.