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
	MOVEIT_CLASS_FORWARD (KinematicConstraint)

	MOVEIT_CLASS_FORWARD (JointConstraint)

	MOVEIT_CLASS_FORWARD (OrientationConstraint)

	MOVEIT_CLASS_FORWARD (PositionConstraint)

	MOVEIT_CLASS_FORWARD (VisibilityConstraint)

	MOVEIT_CLASS_FORWARD (KinematicConstraintSet)

moveit_msgs::msg::Constraints	mergeConstraints (const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
	Merge two sets of constraints into one.

std::size_t	countIndividualConstraints (const moveit_msgs::msg::Constraints &constr)

moveit_msgs::msg::Constraints	constructGoalConstraints (const moveit::core::RobotState &state, const moveit::core::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.

moveit_msgs::msg::Constraints	constructGoalConstraints (const moveit::core::RobotState &state, const moveit::core::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.

bool	updateJointConstraints (moveit_msgs::msg::Constraints &constraints, const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg)
	Update joint constraints with a new JointModelGroup state.

moveit_msgs::msg::Constraints	constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::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.

moveit_msgs::msg::Constraints	constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::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.

bool	updatePoseConstraint (moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::PoseStamped &pose)
	Update a pose constraint for one link with a new pose.

moveit_msgs::msg::Constraints	constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::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.

bool	updateOrientationConstraint (moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::QuaternionStamped &quat)
	Update an orientation constraint for one link with a new quaternion.

moveit_msgs::msg::Constraints	constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::Point &reference_point, const geometry_msgs::msg::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.

moveit_msgs::msg::Constraints	constructGoalConstraints (const std::string &link_name, const geometry_msgs::msg::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.

bool	updatePositionConstraint (moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::PointStamped &goal_point)
	Update a position constraint for one link with a new position.

bool	constructConstraints (const rclcpp::Node::SharedPtr &node, const std::string &constraints_param, moveit_msgs::msg::Constraints &constraints)
	extract constraint message from node parameters.

bool	resolveConstraintFrames (const moveit::core::RobotState &state, moveit_msgs::msg::Constraints &constraints)
	Resolves frames used in constraints to links in the robot model.

template<typename Derived >
std::tuple< Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 >, bool >	calcEulerAngles (const Eigen::MatrixBase< Derived > &R)

Detailed Description

Representation and evaluation of kinematic constraints.

Function Documentation

◆ calcEulerAngles()

template<typename Derived >

std::tuple< Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 >, bool > kinematic_constraints::calcEulerAngles ( const Eigen::MatrixBase< Derived > & R )

This's copied from https://gitlab.com/libeigen/eigen/-/blob/master/unsupported/Eigen/src/EulerAngles/EulerSystem.h#L187 Return the intrinsic Roll-Pitch-Yaw euler angles given the input rotation matrix and boolean indicating whether the there's a singularity in the input rotation matrix (true: the input rotation matrix don't have a singularity, false: the input rotation matrix have a singularity) The returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]

Definition at line 98 of file kinematic_constraint.cpp.

Here is the caller graph for this function:

◆ constructConstraints()

bool kinematic_constraints::constructConstraints	(	const rclcpp::Node::SharedPtr &	node,
		const std::string &	constraints_param,
		moveit_msgs::msg::Constraints &	constraints
	)

extract constraint message from node parameters.

This can be used to construct a Constraints message from specifications provided from a yaml file. An example for a constraint yaml structure (loaded at constraint_param): """ name: constraint_name constraint_ids: [constraint_1, constraint_2] constraints: constraint_1: type: orientation frame_id: world link_name: tool0 orientation: [0, 0, 0] # [r, p, y] tolerances: [0.01, 0.01, 3.15] weight: 1.0 constraint_2: type: position frame_id: base_link link_name: tool0 target_offset: [0.1, 0.1, 0.1] # [x, y, z] region: x: [0.1, 0.4] # [min, max] y: [0.2, 0.3] z: [0.1, 0.6] weight: 1.0 """

Parameters

[in]	params	Node node to load the parameters from
[in]	params	string namespace from where to load the constraints parameters
[out]	constraints	The constructed constraints message

Returns: was the construction successful?

Definition at line 607 of file utils.cpp.

Here is the caller graph for this function:

◆ constructGoalConstraints() [1/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const moveit::core::RobotState &	state,
		const moveit::core::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.

Parameters

[in]	state	The state from which to generate goal joint constraints
[in]	jmg	The group for which to generate joint constraints
[in]	tolerance	An angular tolerance to apply both above and below for all constraints [rad or meters for prismatic joints]

Returns: A full constraint message containing all the joint constraints

Definition at line 146 of file utils.cpp.

Here is the call graph for this function:

◆ constructGoalConstraints() [2/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const moveit::core::RobotState &	state,
		const moveit::core::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.

Parameters

[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 [rad or meters for prismatic joints]
[in]	tolerance_above	The above tolerance to apply to all constraints [rad or meters for prismatic joints]

Returns: A full constraint message containing all the joint constraints

Definition at line 152 of file utils.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ constructGoalConstraints() [3/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const std::string &	link_name,
		const geometry_msgs::msg::Point &	reference_point,
		const geometry_msgs::msg::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.

Parameters

[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 of a sphere defining a PositionConstraint

Returns: A full constraint message containing the position constraint

Definition at line 345 of file utils.cpp.

◆ constructGoalConstraints() [4/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const std::string &	link_name,
		const geometry_msgs::msg::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.

Parameters

[in]	link_name	The link name for the PositionConstraint
[in]	goal_point	The position associated with the constraint region
[in]	tolerance	The radius of a sphere defining a PositionConstraint

Returns: A full constraint message containing the position constraint

Definition at line 312 of file utils.cpp.

Here is the call graph for this function:

◆ constructGoalConstraints() [5/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const std::string &	link_name,
		const geometry_msgs::msg::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.

Parameters

[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

Returns: A full constraint message containing both constraints

Definition at line 232 of file utils.cpp.

Here is the call graph for this function:

◆ constructGoalConstraints() [6/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const std::string &	link_name,
		const geometry_msgs::msg::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.

Parameters

[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 radius of a sphere defining a PositionConstraint
[in]	tolerance_angle	The value to assign to the absolute tolerances of the OrientationConstraint

Returns: A full constraint message containing both constraints

Definition at line 194 of file utils.cpp.

◆ constructGoalConstraints() [7/7]

moveit_msgs::msg::Constraints kinematic_constraints::constructGoalConstraints	(	const std::string &	link_name,
		const geometry_msgs::msg::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.

Parameters

[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

Returns: A full constraint message containing the orientation constraint

Definition at line 275 of file utils.cpp.

◆ countIndividualConstraints()

std::size_t kinematic_constraints::countIndividualConstraints ( const moveit_msgs::msg::Constraints & constr )

Definition at line 140 of file utils.cpp.

◆ mergeConstraints()

moveit_msgs::msg::Constraints kinematic_constraints::mergeConstraints	(	const moveit_msgs::msg::Constraints &	first,
		const moveit_msgs::msg::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

Parameters

[in]	first	The first constraint to merge
[in]	second	The second constraint to merge

Returns: The merged set of constraints

Definition at line 64 of file utils.cpp.

Here is the caller graph for this function:

◆ MOVEIT_CLASS_FORWARD() [1/6]

kinematic_constraints::MOVEIT_CLASS_FORWARD ( JointConstraint )

◆ MOVEIT_CLASS_FORWARD() [2/6]

kinematic_constraints::MOVEIT_CLASS_FORWARD ( KinematicConstraint )

◆ MOVEIT_CLASS_FORWARD() [3/6]

kinematic_constraints::MOVEIT_CLASS_FORWARD ( KinematicConstraintSet )

◆ MOVEIT_CLASS_FORWARD() [4/6]

kinematic_constraints::MOVEIT_CLASS_FORWARD ( OrientationConstraint )

◆ MOVEIT_CLASS_FORWARD() [5/6]

kinematic_constraints::MOVEIT_CLASS_FORWARD ( PositionConstraint )

◆ MOVEIT_CLASS_FORWARD() [6/6]

kinematic_constraints::MOVEIT_CLASS_FORWARD ( VisibilityConstraint )

◆ resolveConstraintFrames()

bool kinematic_constraints::resolveConstraintFrames	(	const moveit::core::RobotState &	state,
		moveit_msgs::msg::Constraints &	constraints
	)

Resolves frames used in constraints to links in the robot model.

The link_name field of a constraint is changed from the name of an object's frame or subframe to the name of the robot link that the object is attached to.

This is used in a planning request adapter which ensures that the planning problem is defined properly (the attached objects' frames are not known to the planner).

Parameters

[in]	state	The RobotState used to resolve frames.
[in]	constraints	The constraint to resolve.

Definition at line 623 of file utils.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateJointConstraints()

bool kinematic_constraints::updateJointConstraints	(	moveit_msgs::msg::Constraints &	constraints,
		const moveit::core::RobotState &	state,
		const moveit::core::JointModelGroup *	jmg
	)

Update joint constraints with a new JointModelGroup state.

Parameters

[in,out]	constraints	Previously-constructed constraints to update
[in]	state	The new target state
[in]	jmg	Specify which JointModelGroup to update

Returns: true if all joint constraints were updated

Definition at line 172 of file utils.cpp.

Here is the call graph for this function:

◆ updateOrientationConstraint()

bool kinematic_constraints::updateOrientationConstraint	(	moveit_msgs::msg::Constraints &	constraints,
		const std::string &	link_name,
		const geometry_msgs::msg::QuaternionStamped &	quat
	)

Update an orientation constraint for one link with a new quaternion.

Parameters

[in,out]	constraints	Previously-constructed constraints to update
[in]	link	The link to update for
[in]	quat	The new target quaternion

Returns: true if the constraint was updated

Definition at line 292 of file utils.cpp.

Here is the caller graph for this function:

◆ updatePoseConstraint()

bool kinematic_constraints::updatePoseConstraint	(	moveit_msgs::msg::Constraints &	constraints,
		const std::string &	link_name,
		const geometry_msgs::msg::PoseStamped &	pose
	)

Update a pose constraint for one link with a new pose.

Parameters

[in,out]	constraints	Previously-constructed constraints to update
[in]	link	The link to update for
[in]	pose	The new target pose

Returns: true if the constraint was updated

Definition at line 257 of file utils.cpp.

Here is the call graph for this function:

◆ updatePositionConstraint()

bool kinematic_constraints::updatePositionConstraint	(	moveit_msgs::msg::Constraints &	constraints,
		const std::string &	link_name,
		const geometry_msgs::msg::PointStamped &	goal_point
	)

Update a position constraint for one link with a new position.

Parameters

[in,out]	constraints	Previously-constructed constraints to update
[in]	link	The link to update for
[in]	goal_point	The new target point

Returns: true if the constraint was updated

Definition at line 323 of file utils.cpp.

Here is the caller graph for this function:

Classes

Functions

Detailed Description

Function Documentation

◆ calcEulerAngles()

◆ constructConstraints()

◆ constructGoalConstraints() [1/7]

◆ constructGoalConstraints() [2/7]

◆ constructGoalConstraints() [3/7]

◆ constructGoalConstraints() [4/7]

◆ constructGoalConstraints() [5/7]

◆ constructGoalConstraints() [6/7]

◆ constructGoalConstraints() [7/7]

◆ countIndividualConstraints()

◆ mergeConstraints()

◆ MOVEIT_CLASS_FORWARD() [1/6]

◆ MOVEIT_CLASS_FORWARD() [2/6]

◆ MOVEIT_CLASS_FORWARD() [3/6]

◆ MOVEIT_CLASS_FORWARD() [4/6]

◆ MOVEIT_CLASS_FORWARD() [5/6]

◆ MOVEIT_CLASS_FORWARD() [6/6]

◆ resolveConstraintFrames()

◆ updateJointConstraints()

◆ updateOrientationConstraint()

◆ updatePoseConstraint()

◆ updatePositionConstraint()