Public Member Functions | Protected Attributes
kinematic_constraints::JointConstraint Class Reference

Class for handling single DOF joint constraints. More...

#include <kinematic_constraint.h>

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

List of all members.

Public Member Functions

virtual void clear ()
 Clear the stored constraint.
bool configure (const moveit_msgs::JointConstraint &jc)
 Configure the constraint based on a moveit_msgs::JointConstraint.
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 joint constraints are the same.
double getDesiredJointPosition () const
 Gets the desired position component of the constraint.
const robot_model::JointModelgetJointModel () const
 Get the joint model for which this constraint operates.
double getJointToleranceAbove () const
 Gets the upper tolerance component of the joint constraint.
double getJointToleranceBelow () const
 Gets the lower tolerance component of the joint constraint.
int getJointVariableIndex () const
const std::string & getJointVariableName () const
 Gets the joint variable name, as known to the robot_model::RobotModel.
const std::string & getLocalVariableName () const
 Gets the local variable name if this constraint was configured for a part of a multi-DOF joint.
 JointConstraint (const robot_model::RobotModelConstPtr &model)
 Constructor.
virtual void print (std::ostream &out=std::cout) const
 Print the constraint data.

Protected Attributes

bool joint_is_continuous_
 Whether or not the joint is continuous.
const robot_model::JointModeljoint_model_
 The joint from the kinematic model for this constraint.
double joint_position_
double joint_tolerance_above_
double joint_tolerance_below_
 Position and tolerance values.
int joint_variable_index_
 The index of the joint variable name in the full robot state.
std::string joint_variable_name_
 The joint variable name.
std::string local_variable_name_
 The local variable name for a multi DOF joint, if any.

Detailed Description

Class for handling single DOF joint constraints.

This class handles single DOF constraints expressed as a tolerance above and below a target position. Multi-DOF joints can be accomodated by using local name formulations - i.e. for a planar joint specifying a constraint in terms of "planar_joint_name"/x.

Continuous revolute single DOF joints will be evaluated based on wrapping around 3.14 and -3.14. Tolerances above and below will be evaluating over the wrap. For instance, if the constraint value is 3.14 and the tolerance above is .04, a value of -3.14 is in bounds, as is a value of -3.12. -3.1 is out of bounds. Similarly, if the value of the constraint is -3.14, the tolerance above is .04, and the tolerance below is .02 then -3.1 is a valid value, as is 3.14; 3.1 is out of bounds.

Type will be JOINT_CONSTRAINT.

Definition at line 203 of file kinematic_constraint.h.


Constructor & Destructor Documentation

kinematic_constraints::JointConstraint::JointConstraint ( const robot_model::RobotModelConstPtr &  model) [inline]

Constructor.

Parameters:
[in]modelThe kinematic model used for constraint evaluation

Definition at line 211 of file kinematic_constraint.h.


Member Function Documentation

Clear the stored constraint.

Implements kinematic_constraints::KinematicConstraint.

Definition at line 244 of file kinematic_constraint.cpp.

bool kinematic_constraints::JointConstraint::configure ( const moveit_msgs::JointConstraint &  jc)

Configure the constraint based on a moveit_msgs::JointConstraint.

For the configure command to be successful, the joint must exist in the kinematic model, the joint must not be a multi-DOF joint (for these joints, local variables should be used), and the tolerance values must be positive.

Parameters:
[in]jcJointConstraint for configuration
Returns:
True if constraint can be configured from jc

Definition at line 71 of file kinematic_constraint.cpp.

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 207 of file kinematic_constraint.cpp.

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 239 of file kinematic_constraint.cpp.

bool kinematic_constraints::JointConstraint::equal ( const KinematicConstraint other,
double  margin 
) const [virtual]

Check if two joint constraints are the same.

This means that the types are the same, the subject of the constraint is the same, and all values associated with the constraint are within a margin. The other constraint must also be enabled. For this to be true of joint constraints, they must act on the same joint, and the position and tolerance values must be within the margins.

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 194 of file kinematic_constraint.cpp.

Gets the desired position component of the constraint.

Returns:
The desired joint position

Definition at line 299 of file kinematic_constraint.h.

Get the joint model for which this constraint operates.

Returns:
The relevant joint model if enabled, and otherwise NULL

Definition at line 259 of file kinematic_constraint.h.

Gets the upper tolerance component of the joint constraint.

Returns:
The above joint tolerance

Definition at line 310 of file kinematic_constraint.h.

Gets the lower tolerance component of the joint constraint.

Returns:
The below joint tolerance

Definition at line 321 of file kinematic_constraint.h.

Definition at line 288 of file kinematic_constraint.h.

const std::string& kinematic_constraints::JointConstraint::getJointVariableName ( ) const [inline]

Gets the joint variable name, as known to the robot_model::RobotModel.

This will include the local variable name if a variable of a multi-DOF joint is constrained.

Returns:
The joint variable name

Definition at line 283 of file kinematic_constraint.h.

const std::string& kinematic_constraints::JointConstraint::getLocalVariableName ( ) const [inline]

Gets the local variable name if this constraint was configured for a part of a multi-DOF joint.

Returns:
The component of the joint name after the slash, or the empty string if there is no local variable name

Definition at line 271 of file kinematic_constraint.h.

void kinematic_constraints::JointConstraint::print ( std::ostream &  out = std::cout) const [virtual]

Print the constraint data.

Parameters:
[in]outThe file descriptor for printing

Reimplemented from kinematic_constraints::KinematicConstraint.

Definition at line 254 of file kinematic_constraint.cpp.


Member Data Documentation

Whether or not the joint is continuous.

Definition at line 328 of file kinematic_constraint.h.

The joint from the kinematic model for this constraint.

Definition at line 327 of file kinematic_constraint.h.

Definition at line 332 of file kinematic_constraint.h.

Definition at line 332 of file kinematic_constraint.h.

Position and tolerance values.

Definition at line 332 of file kinematic_constraint.h.

The index of the joint variable name in the full robot state.

Definition at line 331 of file kinematic_constraint.h.

The joint variable name.

Definition at line 330 of file kinematic_constraint.h.

The local variable name for a multi DOF joint, if any.

Definition at line 329 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 Wed Jun 19 2019 19:23:50