7.3.2. execute_grasp_action_server.ExecuteGraspServer

class execute_grasp_action_server.ExecuteGraspServer

Bases: object

The ExecuteGraspServer class is a ROS action server that plans and executes a grasp pose.

It receives a goal with a list of grasp poses and tries to execute them one by one. If a grasp pose is successfully executed, the object is grasped and the server returns the transform from the object’s bottom surface to the wrist frame. If no grasp pose is successfully executed, the server returns terminates with an aborted status.

The grasp poses are sorted by their orientation and distance. Top grasps are executed last. This means that the closest grasp pose that is not a top grasp is executed first. After trying all non-top grasps, the top grasps are executed in the same order (closest first). This is done because placing objects into shelves is easier when the object is grasped from the side.

The server first moves the robot gripper to a point that is a certain safety distance away from the object, before moving the gripper to the grasp pose. This is to avoid collisions with the object. After grasping the object, the server moves the object up by a certain distance to avoid collisions with the table. Finally, the server retreats in the same direction as the approach to prevent collisions with other objects on the table. The server checks if the grasp was successful by checking the distance between the gripper tips. If the grasp failed the tips are very close to each other and the server aborts the action.

The transformation between the object’s bottom surface and the wrist frame is saved right before the gripper closes around the object. This transformation is needed for placement because the only assumption we can make is that the object is placeable on the surface that it was standing on when it was grasped.

The server uses the MoveitWrapper to plan and execute the robot’s movements inside the collision environment and the HSR_wrapper to plan and execute the robot’s movements without collision checking (and because Moveit has no move_end_effector_by_line function which produces a smooth motion).

tf_wrapper

A wrapper around tf2_ros to transform poses between different frames.

Type:

TF2Wrapper

moveit_wrapper

A wrapper around Moveit to plan and execute the robot’s movements.

Type:

MoveitWrapper

hsr_wrapper

A wrapper around the HSR robot to plan and execute the robot’s movements.

Type:

HSR_wrapper

server

The ROS action server that receives the grasp pose and executes it.

Type:

actionlib.SimpleActionServer

Parameters:

• grasp_poses (list of PoseStamped) – The list of grasp poses to execute.

• grasp_object_name_moveit (string (optional)) – The name of the object to grasp in the Moveit environment. If provided, the object is attached to the gripper after grasping it. The transformation ‘placement_surface_to_wrist’ is only computed when this parameter is provided. This transformation is currently only used for placement.

• table_plane_equations (list of Plane (optional)) – The plane equations of the table surfaces. The table plane equations are used to compute the transformation between the object’s bottom surface and the wrist frame ‘placement_surface_to_wrist’. This transformation is currently only used for placement.

• /safety_distance (float) – The distance to move the gripper away from the object before grasping it.

Returns:

The result of the action server containing the transformation from the object’s bottom surface to the wrist frame (‘placement_surface_to_wrist’) and a boolean indicating if the grasp was a top grasp (‘top_grasp’). The transformation is only computed and returned if ‘grasp_object_name_moveit’ is provided. It is currently only used/needed for placement. The server aborts if no grasp pose was successfully executed or if the grasp failed. If the grasp was successful, the server returns succeeded and the transformation.

Return type:

ExecuteGraspResult

__init__()

Initialize and starts the action server.

Methods

__init__()	Initialize and starts the action server.
execute(goal)	Tries to execute the grasp poses in the goal one by one until a successful grasp is done.
get_transform_from_wrist_to_object_bottom_plane(...)	Compute the transformation from the wrist frame to the object's bottom surface frame.
sort_grasps_by_orientation(grasp_poses)	Sorts grasp poses by their orientation to distinguish between top and not top grasps

execute(goal)

Tries to execute the grasp poses in the goal one by one until a successful grasp is done.

The grasp poses are sorted by their orientation and distance. Top grasps are executed last. This means that the closest grasp pose that is not a top grasp is executed first. After trying all non-top grasps, the top grasps are executed in the same order (closest first). This is done because placing objects into shelves is easier when the object is grasped from the side.

For each grasp pose, the server first moves the robot gripper to a point that is a certain safety distance away from the object, before moving the gripper to the grasp pose. This is to avoid collisions with the object. After grasping the object, the server moves the object up by a certain distance to avoid collisions with the table. Finally, the server retreats in the same direction as the approach to prevent collisions with other objects on the table. The server checks if the grasp was successful by checking the distance between the gripper tips. If the grasp failed the tips are very close to each other and the server aborts the action.

The transformation between the object’s bottom surface and the wrist frame is saved right before the gripper closes around the object. This transformation is needed for placement because the only assumption we can make is that the object is placeable on the surface that it was standing on when it was grasped. The information about whether the grasp was a top grasp is also saved.

Parameters:

goal (ExecuteGraspAction) – The goal containing the grasp poses to execute. Includes the grasp poses, the name of the object to grasp in the Moveit environment and the plane equations of the table surfaces.

get_transform_from_wrist_to_object_bottom_plane(grasp_object_name_moveit, table_plane_equation, planning_frame)

Compute the transformation from the wrist frame to the object’s bottom surface frame.

The transformation is computed by first transforming the object’s pose to the table frame and then computing the object’s bottom surface center in the table frame. The object’s bottom surface center is then aligned with the robot base frame axes. This alignment is done to ensure that the object’s bottom surface z-axis consistently points in the same direction as the robot base frame z-axis (etc. for x and y axes). This way we can consistently compute the transformation between the object’s bottom surface and the wrist frame when doing placement. The transformation between the object’s bottom surface and the wrist frame is then computed.

Parameters:

• goal (ExecuteGraspAction) – The goal containing the grasp poses to execute. Includes the grasp poses, the name of the object to grasp in the Moveit environment and the plane equations of the table surfaces.

• planning_frame (str) – The name of the planning frame in the Moveit environment.

Returns:

The transformation from the object’s bottom surface to the wrist frame.

Return type:

geometry_msgs/Transform

sort_grasps_by_orientation(grasp_poses)

Sorts grasp poses by their orientation to distinguish between top and not top grasps

Parameters:

grasp_poses (geometry_msgs.PoseStamped[]) – Array with grasp poses in the head_rgbd_sensor_rgb_frame

Returns:

Array with the sorted grasp poses. The grasp are sorted by their orientation (not top grasps first, then top grasps) and their distances to the robot (=their original order) list: List with False for not top grasps and True for top grasps

Return type:

geometry_msgs.PoseStamped[]