moveit_studio::behaviors::affordance_transforms Namespace Reference

Functions
std::vector< ObjectSubframe >	calculateGraspAndTwistSubframes (const geometry_msgs::msg::PoseStamped &target_grasp_pose, const double grasp_rotation=15.0)
	Given a grasp pose on an object, calculate subframes that define a screw axis which opens the object along a semicircle in the direction of the grasp pose Z- axis. More...
std::optional< std::vector< moveit_studio_vision_msgs::msg::ObjectSubframe > >	calculateMoveAlongArcSubframes (const geometry_msgs::msg::PoseStamped &pose_grasp, const geometry_msgs::msg::PoseStamped &pose_axis_first, const geometry_msgs::msg::PoseStamped &pose_axis_second)
	Reorder user-selected poses into the subframes in the order expected by affordance template behaviors. More...

Function Documentation

calculateGraspAndTwistSubframes()

std::vector< ObjectSubframe > moveit_studio::behaviors::affordance_transforms::calculateGraspAndTwistSubframes	(	const geometry_msgs::msg::PoseStamped &	target_grasp_pose,
		const double	grasp_rotation = 15.0
	)

Given a grasp pose on an object, calculate subframes that define a screw axis which opens the object along a semicircle in the direction of the grasp pose Z- axis.

Parameters

target_grasp_pose	Input target grasp pose.
grasp_rotation	Input rotation amount, in degrees, by which to rotate the grasp (twist of the end-effector w.r.t. the grasp pose).

Returns

Subframes used to define the screw motion in the affordance template behavior.

calculateMoveAlongArcSubframes()

std::optional< std::vector< ObjectSubframe > > moveit_studio::behaviors::affordance_transforms::calculateMoveAlongArcSubframes	(	const geometry_msgs::msg::PoseStamped &	pose_grasp,
		const geometry_msgs::msg::PoseStamped &	pose_axis_first,
		const geometry_msgs::msg::PoseStamped &	pose_axis_second
	)

Reorder user-selected poses into the subframes in the order expected by affordance template behaviors.

Given three input poses generated from surface-normal calculation of a point cloud, where the first represents the location of the door handle and the second and third are points on the axis of the hinge, determine which hinge axis pose should be the origin of the hinge vector so that a positive rotation about the hinge axis opens the door towards the user's viewpoint.

Assumes that the Z+ axis of the two poses on the screw axis are oriented facing into the surface of the door.

The direction of the door relative to the surface normal is determined by calculating the vector cross product of the vector from the screw origin pose to the grasp pose and the vector from the screw origin pose to the screw axis pose, and then calculating the dot product of the resulting vector and the Z-axis of the screw origin pose. If the dot product is positive, the screw origin and screw axis poses are correctly ordered. If the dot product is negative, the screw origin and screw axis poses need to be reversed for positive door rotation to open the door towards the viewpoint.

Parameters

pose_grasp	Pose representing the grasp point.
pose_axis_first	A pose coincident with the door hinge axis.
pose_axis_second	A second pose coincident with the door hinge axis.

Returns

An optional Subframes vector. Returns nullopt if the Z-axes of the screw origin pose and the screw axis pose are not similarly-oriented (i.e., if the dot product of the two Z-axis vectors is not positive). If successful, returns a Subframe vector containing the three input poses reordered so a positive rotation about a vector originating at the screw origin pose and intersecting the screw axis pose results in the grasp point moving towards the user's viewpoint.