7.5.2. grasp_annotator.GraspAnnotator

class grasp_annotator.GraspAnnotator(pcd_filter_distance=0.16)

Bases: object

__init__(pcd_filter_distance=0.16)

Init function for the GraspAnnotator class. The class is used to check if a grasp is valid by checking if it is reachable and collision free.

Parameters:

pcd_filter_distance (float, optional) – Distance for the filtering of the point cloud in meters. Defaults to 0.16. If 0 or None, the point cloud is not filtered.

Methods

__init__([pcd_filter_distance])

Init function for the GraspAnnotator class.

annotate(object_pose_stamped, scene_pcd, ...)

Goes through all annotated grasps from the .npy file and checks if they are valid.

is_grasp_reachable(grasp_pose, cam_to_base)

Checks if the grasp is reachable based on the angle of approach.

is_grasp_valid(grasp_pose, o3d_pcd, table_plane)

Check if the grasp is valid by checking if it is reachable and collision free.

is_gripper_collision_free(grasp_pose, ...)

Checks if the grasp pose is collision free

point_to_plane_distance(point, plane)

annotate(object_pose_stamped, scene_pcd, object_name, table_plane=None)

Goes through all annotated grasps from the .npy file and checks if they are valid. Returns the valid grasps.

Parameters:

  • object_pose_stamped (geometry_msgs.PoseStamped) – Pose of the object to grasp

  • scene_pcd (sensor_msgs.PointCloud2) – Pointcloud of the current scene created by open3d_ros_helper from the depth image

  • object_name (string) – Name of the object

  • table_plane (np.array, optional) – Parameters of the table plane (4 values). Defaults to None.

Returns:

List of valid grasp poses

Return type:

geometry_msgs.PoseStamped[]

is_grasp_reachable(grasp_pose, cam_to_base)

Checks if the grasp is reachable based on the angle of approach. It rejects grasps that approach objects in the negative x direction (towards the robot body) or in the positive z direction (from below).

Parameters:

  • grasp_pose (np.array) – 4x4 transformation matrix of the grasp in the camera frame

  • cam_to_base (np.array) – 4x4 transformation matrix from the camera to the base

Returns

bool: True if the grasp pose is reachable, false otherwise

is_grasp_valid(grasp_pose, o3d_pcd, table_plane, cam_to_base=None)

Check if the grasp is valid by checking if it is reachable and collision free.

Parameters:

  • grasp_pose (np.array) – 4x4 transformation matrix of the grasp in the camera frame

  • o3d_pcd (open3d.geometry.PointCloud) – Pointcloud of the current scene

  • table_plane (np.array, optional) – Parameters of the table plane (4 values). Defaults to None.

  • cam_to_base (np.array, optional) – 4x4 transformation matrix from the camera to the base

Returns:

True if the grasp pose is valid (reachable and collision free), false otherwise

Return type:

bool

is_gripper_collision_free(grasp_pose, o3d_pcd, table_plane)

Checks if the grasp pose is collision free

Parameters:

  • grasp_pose (np.array) – 4x4 transformation matrix of the grasp in the camera frame

  • o3d_pcd (open3d.geometry.PointCloud) – Pointcloud of the current scene

  • table_plane (np.array, optional) – Parameters of the table plane (4 values). Defaults to None.

Returns

bool: True if the grasp pose is not in collision with the scene and table, false otherwise