Class for constraints on the visibility relationship between a sensor and a target. More...

#include <kinematic_constraint.h>

Inheritance diagram for kinematic_constraints::VisibilityConstraint:

Collaboration diagram for kinematic_constraints::VisibilityConstraint:

Public Member Functions
	VisibilityConstraint (const moveit::core::RobotModelConstPtr &model)
	Constructor.

bool	configure (const moveit_msgs::msg::VisibilityConstraint &vc, const moveit::core::Transforms &tf)
	Configure the constraint based on a moveit_msgs::msg::VisibilityConstraint.

bool	equal (const KinematicConstraint &other, double margin) const override
	Check if two constraints are the same.

void	clear () override
	Clear the stored constraint.

shapes::Mesh *	getVisibilityCone (const Eigen::Isometry3d &tform_world_to_sensor, const Eigen::Isometry3d &tform_world_to_target) const
	Gets a trimesh shape representing the visibility cone.

void	getMarkers (const moveit::core::RobotState &state, visualization_msgs::msg::MarkerArray &markers) const
	Adds markers associated with the visibility cone, sensor and target to the visualization array.

bool	enabled () const override
	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.

ConstraintEvaluationResult	decide (const moveit::core::RobotState &state, bool verbose=false) const override
	Decide whether the constraint is satisfied in the indicated state.

void	print (std::ostream &out=std::cout) const override
	Print the constraint data.

Public Member Functions inherited from kinematic_constraints::KinematicConstraint
	KinematicConstraint (const moveit::core::RobotModelConstPtr &model)
	Constructor.

virtual	~KinematicConstraint ()

ConstraintType	getType () const
	Get the type of constraint.

double	getConstraintWeight () const
	The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() function.

const moveit::core::RobotModelConstPtr &	getRobotModel () const

Protected Member Functions
bool	decideContact (const collision_detection::Contact &contact) const
	Function that gets passed into collision checking to allow some collisions.

Protected Attributes
moveit::core::RobotModelConstPtr	robot_model_
	A copy of the robot model used to create collision environments to check the cone against robot links.

std::string	target_frame_id_
	The target frame id.

std::string	sensor_frame_id_
	The sensor frame id.

Eigen::Isometry3d	sensor_pose_
	The sensor pose transformed into the transform frame.

int	sensor_view_direction_
	Storage for the sensor view direction.

Eigen::Isometry3d	target_pose_
	The target pose transformed into the transform frame.

unsigned int	cone_sides_
	Storage for the cone sides

EigenSTL::vector_Vector3d	points_
	A set of points along the base of the circle.

double	target_radius_
	Storage for the target radius.

double	max_view_angle_
	Storage for the max view angle.

double	max_range_angle_
	Storage for the max range angle.

Protected Attributes inherited from kinematic_constraints::KinematicConstraint
ConstraintType	type_
	The type of the constraint.

moveit::core::RobotModelConstPtr	robot_model_
	The kinematic model associated with this constraint.

double	constraint_weight_
	The weight of a constraint is a multiplicative factor associated to the distance computed by the decide() function

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 visibility relationship between a sensor and a target.

Visibility constraints are the most complicated kinematic constraint type. They are designed to test whether a visibility relationship is maintained in a given state. For the visibility relationship to be maintained, a sensor must have an unimpeded view of a target - this is useful, for instance, if one wants to test whether a robot can see its hand with a given sensor in a given state. The mechanism that is used to test the constraint is a combination of geometric checks, and then a collision check that tests whether a cone that connects the sensor and the target is entirely unobstructed by the robot's links.

The constraint consists of a sensor pose, a target pose, a few parameters that govern the shape of the target, and a few parameters that allow finer control over the geometry of the check. There are two general ways that this constraint can be used. The first way to leave max_view_angle and max_range_angle in the VisibilityConstraint message as 0.0. In this case, the only check that is performed is the collision check against the cone. This check assumes that the sensor can see in all directions - the orientation of the sensor has no effect on the creation of the cone. It is important to note that the sensor and the target poses must not result in collision - if they are associated with sensor frames or robot links they must be specified to be outside the robot's body or all states will violate the Visibility Constraint. For an example of a visualization of a visibility constraint (produced using the getMarkers member function), see this image:

Visibility constraint satisfied as cone volume is clear

The cone is shown in red, and the arrows show normals. This visibility constraint uses a sensor pose (the narrow end of the cone ) of the PR2's narrow_stereo_optical_frame, except shifted along the Z axis such that the pose is outside of the robot's head and doesn't collide. The cone is allowed to collide with the actual sensor associated with the header frame, just not with any other links. The target pose is a 5 cm circle offset from the l_gripper_r_finger_tip_link, again so as not to collide - again, the cone can collide with the target link, but now with any other links. In the indicated state the cone is collision free and thus satisfies the collision component of the visibility constraint. In this image, however, the right arm intersects the cone, violating the visibility constraint:

Visibility constraint violated as right arm is within the cone

Note that both the target and the sensor frame can change with the robot's state.

The collision check essentially asks whether or not the volume between the sensor pose and the target pose are clear, but that's only one aspect of visibility we may care about. The visibility constraint also allows for some geometric reasoning about the relationship between the sensor or the target - this allows information such as approximate field of view of the sensor to be factored into the constraint. To use this reasoning, the sensor_view_direction of the field should first be set - this specifies which axis of the sensor pose frame is actually pointing. The system assumes that the the sensor pose has been set up such that a single of the axes is pointing directly out of the sensor. Once this value is set correctly, one or both of the max_view_angle and the max_range_angle values can be set. Setting a positive max_view_angle will constrain the sensor along the specified axis and the target along its Z axis to be pointing at each other. Practically speaking, this ensures that the sensor has sufficient visibility to the front of the target - if the target is pointing the opposite direction, or is too steeply perpendicular to the target, then the max_view_angle part of the constraint will be violated. The getMarkers function can again help explain this - the view angle is the angular difference between the blue arrow associated with the sensor and the red arrow associated with the target.

Max view angle is evaluated at 0.0

Max view angle evaluates around pi/4

Max view angle evaluates at pi/2, the maximum

Sensor pointed at wrong side of target, will violate constraint as long as max_view_angle

‍0.0" If constraining the target to be within the field of view of the sensor is the goal, then the max_range_angle can be set. The range angle is calculated as the angle between the origins of the sensor and the target frame - the orientations are unimportant. In the above images, the range angle is always 0, as the target's center is directly lined up with the blue arrow. In this image, however, the view angle is evaluated at 0.0, while the range angle is evaluated at .65. @image html range_angle.png "Range angle is high, so only sensors with wide fields of view would see the target"

By limiting the max_range_angle, you can constrain the target to be within the field of view of the sensor. Max_view_angle and max_range_angle can be used at once.

Definition at line 779 of file kinematic_constraint.h.

Constructor & Destructor Documentation

◆ VisibilityConstraint()

kinematic_constraints::VisibilityConstraint::VisibilityConstraint ( const moveit::core::RobotModelConstPtr & model )

Constructor.

Parameters

[in] model The kinematic model used for constraint evaluation

Definition at line 794 of file kinematic_constraint.cpp.

Member Function Documentation

◆ clear()

void kinematic_constraints::VisibilityConstraint::clear ( )

overridevirtual

Clear the stored constraint.

Implements kinematic_constraints::KinematicConstraint.

Definition at line 800 of file kinematic_constraint.cpp.

Here is the caller graph for this function:

◆ configure()

bool kinematic_constraints::VisibilityConstraint::configure	(	const moveit_msgs::msg::VisibilityConstraint &	vc,
		const moveit::core::Transforms &	tf
	)

Configure the constraint based on a moveit_msgs::msg::VisibilityConstraint.

For the configure command to be successful, one of the three possible constraint criteria must be set to a non-zero value:

The target radius (negative values will have the absolute value taken).
The range angle.
The view angle.

If cone sides are less than 3, a value of 3 will be used.

Parameters

[in] vc moveit_msgs::msg::VisibilityConstraint for configuration

Returns: True if constraint can be configured from vc

Definition at line 814 of file kinematic_constraint.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ decide()

ConstraintEvaluationResult kinematic_constraints::VisibilityConstraint::decide	(	const moveit::core::RobotState &	state,
		bool	verbose = `false`
	)		const

overridevirtual

Decide whether the constraint is satisfied in the indicated state.

Parameters

[in]	state	The kinematic state used for evaluation
[in]	verbose	Whether or not to print output

Returns

Implements kinematic_constraints::KinematicConstraint.

Definition at line 1069 of file kinematic_constraint.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ decideContact()

bool kinematic_constraints::VisibilityConstraint::decideContact ( const collision_detection::Contact & contact ) const

protected

Function that gets passed into collision checking to allow some collisions.

The cone object is allowed to touch either the sensor or the header frame, but not anything else

Parameters

[in] contact The contact in question

Returns: True if the collision is allowed, otherwise false

Definition at line 1185 of file kinematic_constraint.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ enabled()

bool kinematic_constraints::VisibilityConstraint::enabled ( ) const

overridevirtual

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

Here is the caller graph for this function:

◆ equal()

bool kinematic_constraints::VisibilityConstraint::equal	(	const KinematicConstraint &	other,
		double	margin
	)		const

overridevirtual

Check if two constraints are the same.

For visibility constraints this means that:

The types are the same,
The target frame ids are the same
The sensor frame ids are the same
The cone sides number is the same
The sensor view directions are the same
The max view angles and target radii are within the margin
The sensor and target poses are within the margin, as computed by taking the norm of the difference.

Parameters

[in]	other	The other constraint to test
[in]	margin	The margin to apply to all values associated with constraint

Returns: True if equal, otherwise false

Implements kinematic_constraints::KinematicConstraint.

Definition at line 886 of file kinematic_constraint.cpp.

Here is the call graph for this function:

◆ getMarkers()

void kinematic_constraints::VisibilityConstraint::getMarkers	(	const moveit::core::RobotState &	state,
		visualization_msgs::msg::MarkerArray &	markers
	)		const

Adds markers associated with the visibility cone, sensor and target to the visualization array.

The visibility cone and two arrows - a blue array that issues from the sensor_view_direction of the sensor, and a red arrow the issues along the Z axis of the the target frame.

Parameters

[in]	state	The state from which to produce the markers
[out]	markers	The marker array to which the markers will be added

Definition at line 996 of file kinematic_constraint.cpp.

Here is the call graph for this function:

◆ getVisibilityCone()

shapes::Mesh * kinematic_constraints::VisibilityConstraint::getVisibilityCone	(	const Eigen::Isometry3d &	tform_world_to_sensor,
		const Eigen::Isometry3d &	tform_world_to_target
	)		const

Gets a trimesh shape representing the visibility cone.

Parameters

[in]	tform_world_to_sensor	Transform from the world to the sensor frame
[in]	tform_world_to_target	Transform from the world to the target frame

Returns: The shape associated with the cone

Definition at line 922 of file kinematic_constraint.cpp.

Here is the caller graph for this function:

◆ print()

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

overridevirtual

Print the constraint data.

Parameters

[in] out The file descriptor for printing

Reimplemented from kinematic_constraints::KinematicConstraint.

Definition at line 1209 of file kinematic_constraint.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ cone_sides_

unsigned int kinematic_constraints::VisibilityConstraint::cone_sides_

protected

Storage for the cone sides

Definition at line 878 of file kinematic_constraint.h.

◆ max_range_angle_

double kinematic_constraints::VisibilityConstraint::max_range_angle_

protected

Storage for the max range angle.

Definition at line 882 of file kinematic_constraint.h.

◆ max_view_angle_

double kinematic_constraints::VisibilityConstraint::max_view_angle_

protected

Storage for the max view angle.

Definition at line 881 of file kinematic_constraint.h.

◆ points_

EigenSTL::vector_Vector3d kinematic_constraints::VisibilityConstraint::points_

protected

A set of points along the base of the circle.

Definition at line 879 of file kinematic_constraint.h.

◆ robot_model_

moveit::core::RobotModelConstPtr kinematic_constraints::VisibilityConstraint::robot_model_

protected

A copy of the robot model used to create collision environments to check the cone against robot links.

Definition at line 870 of file kinematic_constraint.h.

◆ sensor_frame_id_

std::string kinematic_constraints::VisibilityConstraint::sensor_frame_id_

protected

The sensor frame id.

Definition at line 874 of file kinematic_constraint.h.

◆ sensor_pose_

Eigen::Isometry3d kinematic_constraints::VisibilityConstraint::sensor_pose_

protected

The sensor pose transformed into the transform frame.

Definition at line 875 of file kinematic_constraint.h.

◆ sensor_view_direction_

int kinematic_constraints::VisibilityConstraint::sensor_view_direction_

protected

Storage for the sensor view direction.

Definition at line 876 of file kinematic_constraint.h.

◆ target_frame_id_

std::string kinematic_constraints::VisibilityConstraint::target_frame_id_

protected

The target frame id.

Definition at line 873 of file kinematic_constraint.h.

◆ target_pose_

Eigen::Isometry3d kinematic_constraints::VisibilityConstraint::target_pose_

protected

The target pose transformed into the transform frame.

Definition at line 877 of file kinematic_constraint.h.

◆ target_radius_

double kinematic_constraints::VisibilityConstraint::target_radius_

protected

Storage for the target radius.

Definition at line 880 of file kinematic_constraint.h.

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

moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h
moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ VisibilityConstraint()

Member Function Documentation

◆ clear()

◆ configure()

◆ decide()

◆ decideContact()

◆ enabled()

◆ equal()

◆ getMarkers()

◆ getVisibilityCone()

◆ print()

Member Data Documentation

◆ cone_sides_

◆ max_range_angle_

◆ max_view_angle_

◆ points_

◆ robot_model_

◆ sensor_frame_id_

◆ sensor_pose_

◆ sensor_view_direction_

◆ target_frame_id_

◆ target_pose_

◆ target_radius_