Representation and evaluation of kinematic constraints. More...
Classes | |
struct | ConstraintEvaluationResult |
Struct for containing the results of constraint evaluation. More... | |
class | JointConstraint |
Class for handling single DOF joint constraints. More... | |
class | KinematicConstraint |
Base class for representing a kinematic constraint. More... | |
class | KinematicConstraintSet |
A class that contains many different constraints, and can check RobotState *versus the full set. A set is satisfied if and only if all constraints are satisfied. More... | |
class | OrientationConstraint |
Class for constraints on the orientation of a link. More... | |
class | PositionConstraint |
Class for constraints on the XYZ position of a link. More... | |
class | VisibilityConstraint |
Class for constraints on the visibility relationship between a sensor and a target. More... | |
Functions | |
static bool | collectConstraints (XmlRpc::XmlRpcValue ¶ms, moveit_msgs::Constraints &constraints) |
static bool | constructConstraint (XmlRpc::XmlRpcValue ¶ms, moveit_msgs::JointConstraint &constraint) |
static bool | constructConstraint (XmlRpc::XmlRpcValue ¶ms, moveit_msgs::PositionConstraint &constraint) |
static bool | constructConstraint (XmlRpc::XmlRpcValue ¶ms, moveit_msgs::OrientationConstraint &constraint) |
static bool | constructConstraint (XmlRpc::XmlRpcValue ¶ms, moveit_msgs::VisibilityConstraint &constraint) |
bool | constructConstraints (XmlRpc::XmlRpcValue ¶ms, moveit_msgs::Constraints &constraints) |
extract constraint message from XmlRpc node. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above) |
Generates a constraint message intended to be used as a goal constraint for a joint group. The full constraint will contain a vector of type JointConstraint, one for each DOF in the group. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon()) |
Generates a constraint message intended to be used as a goal constraint for a joint group. The full constraint will contain a vector of type JointConstraint, one for each DOF in the group. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const std::string &link_name, const geometry_msgs::PoseStamped &pose, double tolerance_pos=1e-3, double tolerance_angle=1e-2) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint will contain a PositionConstraint and a OrientationConstraint, constructed from the pose. A sphere will be used to represent the constraint region for the PositionConstraint. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const std::string &link_name, const geometry_msgs::PoseStamped &pose, const std::vector< double > &tolerance_pos, const std::vector< double > &tolerance_angle) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint will contain a PositionConstraint and a OrientationConstraint, constructed from the pose. A box will be used to represent the constraint region for the PositionConstraint. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const std::string &link_name, const geometry_msgs::QuaternionStamped &quat, double tolerance=1e-2) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only an OrientationConstraint. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const std::string &link_name, const geometry_msgs::Point &reference_point, const geometry_msgs::PointStamped &goal_point, double tolerance=1e-3) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only a PositionConstraint. A sphere will be used to represent the constraint region. More... | |
moveit_msgs::Constraints | constructGoalConstraints (const std::string &link_name, const geometry_msgs::PointStamped &goal_point, double tolerance=1e-3) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only a PositionConstraint. A sphere will be used to represent the constraint region. More... | |
static bool | constructPoseStamped (XmlRpc::XmlRpcValue::iterator &it, geometry_msgs::PoseStamped &pose) |
std::size_t | countIndividualConstraints (const moveit_msgs::Constraints &constr) |
static ConstraintEvaluationResult | finishPositionConstraintDecision (const Eigen::Vector3d &pt, const Eigen::Vector3d &desired, const std::string &name, double weight, bool result, bool verbose) |
static bool | isArray (XmlRpc::XmlRpcValue &v, size_t size, const std::string &name="", const std::string &description="") |
bool | isEmpty (const moveit_msgs::Constraints &constr) |
Check if any constraints were specified. More... | |
static bool | isStruct (XmlRpc::XmlRpcValue &v, const std::set< std::string > &keys, const std::string &name="") |
moveit_msgs::Constraints | mergeConstraints (const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second) |
Merge two sets of constraints into one. More... | |
MOVEIT_CLASS_FORWARD (KinematicConstraint) | |
MOVEIT_CLASS_FORWARD (JointConstraint) | |
MOVEIT_CLASS_FORWARD (OrientationConstraint) | |
MOVEIT_CLASS_FORWARD (PositionConstraint) | |
MOVEIT_CLASS_FORWARD (VisibilityConstraint) | |
MOVEIT_CLASS_FORWARD (KinematicConstraintSet) | |
static double | normalizeAngle (double angle) |
static double | parseDouble (XmlRpc::XmlRpcValue &v) |
Variables | |
const std::string | LOGNAME = "kinematic_constraint_utils" |
Representation and evaluation of kinematic constraints.
|
static |
|
static |
|
static |
|
static |
|
static |
bool kinematic_constraints::constructConstraints | ( | XmlRpc::XmlRpcValue & | params, |
moveit_msgs::Constraints & | constraints | ||
) |
extract constraint message from XmlRpc node.
This can be used to construct a Constraints message from specifications uploaded on the parameter server.
[in] | params | XmlRpc node of the parameter specification |
[out] | constraints | The constructed constraints message |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const robot_state::RobotState & | state, |
const robot_model::JointModelGroup * | jmg, | ||
double | tolerance_below, | ||
double | tolerance_above | ||
) |
Generates a constraint message intended to be used as a goal constraint for a joint group. The full constraint will contain a vector of type JointConstraint, one for each DOF in the group.
[in] | state | The state from which to generate goal joint constraints |
[in] | jmg | The group for which to generate goal joint constraints |
[in] | tolerance_below | The below tolerance to apply to all constraints |
[in] | tolerance_above | The above tolerance to apply to all constraints |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const robot_state::RobotState & | state, |
const robot_model::JointModelGroup * | jmg, | ||
double | tolerance = std::numeric_limits<double>::epsilon() |
||
) |
Generates a constraint message intended to be used as a goal constraint for a joint group. The full constraint will contain a vector of type JointConstraint, one for each DOF in the group.
[in] | state | The state from which to generate goal joint constraints |
[in] | jmg | The group for which to generate joint constraints |
[in] | tolerance | A tolerance to apply both above and below for all constraints |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const std::string & | link_name, |
const geometry_msgs::PoseStamped & | pose, | ||
double | tolerance_pos = 1e-3 , |
||
double | tolerance_angle = 1e-2 |
||
) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint will contain a PositionConstraint and a OrientationConstraint, constructed from the pose. A sphere will be used to represent the constraint region for the PositionConstraint.
[in] | link_name | The link name for both constraints |
[in] | pose | The pose stamped to be used for the target region. |
[in] | tolerance_pos | The dimension of the sphere associated with the target region of the PositionConstraint |
[in] | tolerance_angle | The value to assign to the absolute tolerances of the OrientationConstraint |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const std::string & | link_name, |
const geometry_msgs::PoseStamped & | pose, | ||
const std::vector< double > & | tolerance_pos, | ||
const std::vector< double > & | tolerance_angle | ||
) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint will contain a PositionConstraint and a OrientationConstraint, constructed from the pose. A box will be used to represent the constraint region for the PositionConstraint.
[in] | link_name | The link name for both constraints |
[in] | pose | The pose stamped to be used for the target region. |
[in] | tolerance_pos | The dimensions of the box (xyz) associated with the target region of the PositionConstraint |
[in] | tolerance_angle | The values to assign to the absolute tolerances (xyz) of the OrientationConstraint |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const std::string & | link_name, |
const geometry_msgs::QuaternionStamped & | quat, | ||
double | tolerance = 1e-2 |
||
) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only an OrientationConstraint.
[in] | link_name | The link name for the OrientationConstraint |
[in] | quat | The quaternion for the OrientationConstraint |
[in] | tolerance | The absolute axes tolerances to apply to the OrientationConstraint |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const std::string & | link_name, |
const geometry_msgs::Point & | reference_point, | ||
const geometry_msgs::PointStamped & | goal_point, | ||
double | tolerance = 1e-3 |
||
) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only a PositionConstraint. A sphere will be used to represent the constraint region.
[in] | link_name | The link name for the PositionConstraint |
[in] | reference_point | A point corresponding to the target_point_offset of the PositionConstraint |
[in] | goal_point | The position associated with the constraint region |
[in] | tolerance | The radius associated with the sphere volume associated with the constraint region |
moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints | ( | const std::string & | link_name, |
const geometry_msgs::PointStamped & | goal_point, | ||
double | tolerance = 1e-3 |
||
) |
Generates a constraint message intended to be used as a goal constraint for a given link. The full constraint message will contain only a PositionConstraint. A sphere will be used to represent the constraint region.
[in] | link_name | The link name for the PositionConstraint |
[in] | goal_point | The position associated with the constraint region |
[in] | tolerance | The radius associated with the sphere volume associated with the constraint region |
|
static |
std::size_t kinematic_constraints::countIndividualConstraints | ( | const moveit_msgs::Constraints & | constr | ) |
|
inlinestatic |
Definition at line 415 of file kinematic_constraint.cpp.
|
static |
bool kinematic_constraints::isEmpty | ( | const moveit_msgs::Constraints & | constr | ) |
|
static |
moveit_msgs::Constraints kinematic_constraints::mergeConstraints | ( | const moveit_msgs::Constraints & | first, |
const moveit_msgs::Constraints & | second | ||
) |
Merge two sets of constraints into one.
This just does appending of all constraints except joint constraints. For members of type JointConstraint, the bounds specified in the parameter first take precedence over parameter second
[in] | first | The first constraint to merge |
[in] | second | The second constraint to merge |
kinematic_constraints::MOVEIT_CLASS_FORWARD | ( | KinematicConstraint | ) |
kinematic_constraints::MOVEIT_CLASS_FORWARD | ( | JointConstraint | ) |
kinematic_constraints::MOVEIT_CLASS_FORWARD | ( | OrientationConstraint | ) |
kinematic_constraints::MOVEIT_CLASS_FORWARD | ( | PositionConstraint | ) |
kinematic_constraints::MOVEIT_CLASS_FORWARD | ( | VisibilityConstraint | ) |
kinematic_constraints::MOVEIT_CLASS_FORWARD | ( | KinematicConstraintSet | ) |
|
static |
Definition at line 51 of file kinematic_constraint.cpp.
|
static |