Policies

All GQ-CNN grasping policies are child classes of the base GraspingPolicy class that implements __call__(), which operates on RgbdImageStates and returns a GraspAction.

$ from gqcnn import RgbdImageState, CrossEntropyRobustGraspingPolicy
$
$ im = RgbdImageState.load(<saved rgbd image dir>)
$ my_policy = CrossEntropyRobustGraspingPolicy(<policy initializer args>)
$
$ my_grasp_action = my_policy(im)

Primary Policies

CrossEntropyRobustGraspingPolicy

An implementation of the Cross Entropy Method (CEM) used in Dex-Net 2.0, Dex-Net 2.1, Dex-Net 3.0, and Dex-Net 4.0 to iteratively locate the best grasp.

class gqcnn.CrossEntropyRobustGraspingPolicy(config, filters=None)

Bases: gqcnn.grasping.policy.policy.GraspingPolicy

Optimizes a set of grasp candidates in image space using the cross entropy method.

Cross entropy method (CEM): (1) sample an initial set of candidates (2) sort the candidates (3) fit a GMM to the top P% (4) re-sample grasps from the distribution (5) repeat steps 2-4 for K iters (6) return the best candidate from the final sample set

__init__(config, filters=None)
Parameters
  • config (dict) – Python dictionary of policy configuration parameters.

  • filters (dict) – Python dictionary of functions to apply to filter invalid grasps.

Notes

Required configuration dictionary parameters are specified in Other Parameters.

Other Parameters
  • num_seed_samples (int) – Number of candidate to sample in the initial set.

  • num_gmm_samples (int) – Number of candidates to sample on each resampling from the GMMs.

  • num_iters (int) – Number of sample-and-refit iterations of CEM.

  • gmm_refit_p (float) – Top p-% of grasps used for refitting.

  • gmm_component_frac (float) – Percentage of the elite set size used to determine number of GMM components.

  • gmm_reg_covar (float) – Regularization parameters for GMM covariance matrix, enforces diversity of fitted distributions.

  • deterministic (bool, optional) – Whether to set the random seed to enforce deterministic behavior.

  • gripper_width (float, optional) – Width of the gripper in meters.

select(grasps, q_values)

Selects the grasp with the highest probability of success.

Can override for alternate policies (e.g. epsilon greedy).

Parameters
  • grasps (list) – Python list of gqcnn.grasping.Grasp2D or gqcnn.grasping.SuctionPoint2D grasps to select from.

  • q_values (list) – Python list of associated q-values.

Returns

Grasp with highest probability of success.

Return type

gqcnn.grasping.Grasp2D or gqcnn.grasping.SuctionPoint2D

action_set(state)

Plan a set of grasps with the highest probability of success on the given RGB-D image.

Parameters

state (RgbdImageState) – Image to plan grasps on.

Returns

Grasps to execute.

Return type

python list of gqcnn.grasping.Grasp2D or gqcnn.grasping.SuctionPoint2D # noqa E501

FullyConvolutionalGraspingPolicyParallelJaw

An implementation of the FC-GQ-CNN parallel jaw policy that uses dense, parallelized fully convolutional networks.

class gqcnn.FullyConvolutionalGraspingPolicyParallelJaw(cfg, filters=None)

Bases: gqcnn.grasping.policy.fc_policy.FullyConvolutionalGraspingPolicy

Parallel jaw grasp sampling policy using the FC-GQ-CNN.

__init__(cfg, filters=None)
Parameters
  • cfg (dict) – Python dictionary of policy configuration parameters.

  • filters (dict) – Python dictionary of functions to apply to filter invalid grasps.

FullyConvolutionalGraspingPolicySuction

An implementation of the FC-GQ-CNN suction policy that uses dense, parallelized fully convolutional networks.

class gqcnn.FullyConvolutionalGraspingPolicySuction(cfg, filters=None)

Bases: gqcnn.grasping.policy.fc_policy.FullyConvolutionalGraspingPolicy

Suction grasp sampling policy using the FC-GQ-CNN.

Grasps and Image Wrappers

RgbdImageState

A wrapper for states containing an RGBD (RGB + Depth) image, camera intrinisics, and segmentation masks.

class gqcnn.RgbdImageState(rgbd_im, camera_intr, segmask=None, obj_segmask=None, fully_observed=None)

Bases: object

State to encapsulate RGB-D images.

__init__(rgbd_im, camera_intr, segmask=None, obj_segmask=None, fully_observed=None)
Parameters
  • rgbd_im (perception.RgbdImage) – An RGB-D image to plan grasps on.

  • camera_intr (perception.CameraIntrinsics) – Intrinsics of the RGB-D camera.

  • segmask (perception.BinaryImage) – Segmentation mask for the image.

  • obj_segmask (perception.SegmentationImage) – Segmentation mask for the different objects in the image.

  • full_observed (object) – Representation of the fully observed state.

save(save_dir)

Save to a directory.

Parameters

save_dir (str) – The directory to save to.

static load(save_dir)

Load an RGBDImageState.

Parameters

save_dir (str) – The directory to load from.

GraspAction

A wrapper for 2D grasp actions such as Grasp2D or SuctionPoint2D.

class gqcnn.grasping.policy.policy.GraspAction(grasp, q_value, image=None, policy_name=None)

Bases: object

Action to encapsulate grasps.

__init__(grasp, q_value, image=None, policy_name=None)
Parameters
  • grasp (:obj`Grasp2D` or SuctionPoint2D) – 2D grasp to wrap.

  • q_value (float) – Grasp quality.

  • image (perception.DepthImage) – Depth image corresponding to grasp.

  • policy_name (str) – Policy name.

save(save_dir)

Save grasp action.

Parameters

save_dir (str) – Directory to save the grasp action to.

static load(save_dir)

Load a saved grasp action.

Parameters

save_dir (str) – Directory of the saved grasp action.

Returns

Loaded grasp action.

Return type

GraspAction

Grasp2D

A wrapper for 2D parallel jaw grasps.

class gqcnn.grasping.grasp.Grasp2D(center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, contact_points=None, contact_normals=None)

Bases: object

Parallel-jaw grasp in image space.

center

Point in image space.

Type

autolab_core.Point

angle

Grasp axis angle with the camera x-axis.

Type

float

depth

Depth of the grasp center in 3D space.

Type

float

width

Distance between the jaws in meters.

Type

float

camera_intr

Frame of reference for camera that the grasp corresponds to.

Type

perception.CameraIntrinsics

contact_points

Pair of contact points in image space.

Type

list of numpy.ndarray

contact_normals

Pair of contact normals in image space.

Type

list of numpy.ndarray

property axis

Returns the grasp axis.

property approach_angle

The angle between the grasp approach axis and camera optical axis.

property frame

The name of the frame of reference for the grasp.

property width_px

Returns the width in pixels.

property endpoints

Returns the grasp endpoints.

property feature_vec

Returns the feature vector for the grasp.

v = [p1, p2, depth] where p1 and p2 are the jaw locations in image space.

static from_feature_vec(v, width=0.0, camera_intr=None)

Creates a Grasp2D instance from a feature vector and additional parameters.

Parameters
  • v (numpy.ndarray) – Feature vector, see Grasp2D.feature_vec.

  • width (float) – Grasp opening width, in meters.

  • camera_intr (perception.CameraIntrinsics) – Frame of reference for camera that the grasp corresponds to.

pose(grasp_approach_dir=None)

Computes the 3D pose of the grasp relative to the camera.

If an approach direction is not specified then the camera optical axis is used.

Parameters

grasp_approach_dir (numpy.ndarray) – Approach direction for the grasp in camera basis (e.g. opposite to table normal).

Returns

The transformation from the grasp to the camera frame of reference.

Return type

autolab_core.RigidTransform

static image_dist(g1, g2, alpha=1.0)

Computes the distance between grasps in image space.

Uses Euclidean distance with alpha weighting of angles

Parameters
  • g1 (Grasp2D) – First grasp.

  • g2 (Grasp2D) – Second grasp.

  • alpha (float) – Weight of angle distance (rad to meters).

Returns

Distance between grasps.

Return type

float

SuctionPoint2D

A wrapper for 2D suction grasps.

class gqcnn.grasping.grasp.SuctionPoint2D(center, axis=None, depth=1.0, camera_intr=None)

Bases: object

Suction grasp in image space.

center

Point in image space.

Type

autolab_core.Point

axis

Dormalized 3-vector representing the direction of the suction tip.

Type

numpy.ndarray

depth

Depth of the suction point in 3D space.

Type

float

camera_intr

Frame of reference for camera that the suction point corresponds to.

Type

perception.CameraIntrinsics

property frame

The name of the frame of reference for the grasp.

property angle

The angle that the grasp pivot axis makes in image space.

property approach_angle

The angle between the grasp approach axis and camera optical axis.

property feature_vec

Returns the feature vector for the suction point.

Note

v = [center, axis, depth]

static from_feature_vec(v, camera_intr=None, depth=None, axis=None)

Creates a SuctionPoint2D instance from a feature vector and additional parameters.

Parameters
  • v (numpy.ndarray) – Feature vector, see Grasp2D.feature_vec.

  • camera_intr (perception.CameraIntrinsics) – Frame of reference for camera that the grasp corresponds to.

  • depth (float) – Hard-set the depth for the suction grasp.

  • axis (numpy.ndarray) – Normalized 3-vector specifying the approach direction.

pose()

Computes the 3D pose of the grasp relative to the camera.

Returns

The transformation from the grasp to the camera frame of reference.

Return type

autolab_core.RigidTransform

static image_dist(g1, g2, alpha=1.0)

Computes the distance between grasps in image space.

Uses Euclidean distance with alpha weighting of angles.

Parameters
  • g1 (SuctionPoint2D) – First suction point.

  • g2 (SuctionPoint2D) – Second suction point.

  • alpha (float) – Weight of angle distance (rad to meters).

Returns

Distance between grasps.

Return type

float

Miscellaneous and Parent Policies

Policy

class gqcnn.grasping.policy.policy.Policy

Bases: abc.ABC

Abstract policy class.

__call__(state)

Execute the policy on a state.

abstract action(state)

Returns an action for a given state.

GraspingPolicy

class gqcnn.grasping.policy.policy.GraspingPolicy(config, init_sampler=True)

Bases: gqcnn.grasping.policy.policy.Policy

Policy for robust grasping with Grasp Quality Convolutional Neural Networks (GQ-CNN).

__init__(config, init_sampler=True)
Parameters
  • config (dict) – Python dictionary of parameters for the policy.

  • init_sampler (bool) – Whether or not to initialize the grasp sampler.

Notes

Required configuration parameters are specified in Other Parameters.

Other Parameters
  • sampling (dict) – Dictionary of parameters for grasp sampling, see gqcnn/grasping/image_grasp_sampler.py.

  • gqcnn_model (str) – String path to a trained GQ-CNN model.

property config

Returns the policy configuration parameters.

Returns

Python dictionary of the policy configuration parameters.

Return type

dict

property grasp_sampler

Returns the grasp sampler.

Returns

The grasp sampler.

Return type

gqcnn.grasping.image_grasp_sampler.ImageGraspSampler

property grasp_quality_fn

Returns the grasp quality function.

Returns

The grasp quality function.

Return type

gqcnn.grasping.grasp_quality_function.GraspQualityFunction

property grasp_constraint_fn

Returns the grasp constraint function.

Returns

The grasp contraint function.

Return type

gqcnn.grasping.constraint_fn.GraspConstraintFn

property gqcnn

Returns the GQ-CNN.

Returns

The GQ-CNN model.

Return type

gqcnn.model.tf.GQCNNTF

set_constraint_fn(constraint_fn)

Sets the grasp constraint function.

Parameters

constraint_fn (:obj`gqcnn.grasping.constraint_fn.GraspConstraintFn`) – The grasp contraint function.

action(state)

Returns an action for a given state.

Parameters

state (RgbdImageState) – The RGB-D image state to plan grasps on.

Returns

The planned grasp action.

Return type

GraspAction

show(filename=None, dpi=100)

Show a figure.

Parameters
  • filename (str) – File to save figure to.

  • dpi (int) – Dpi of figure.

FullyConvolutionalGraspingPolicy

class gqcnn.grasping.policy.fc_policy.FullyConvolutionalGraspingPolicy(cfg, filters=None)

Bases: gqcnn.grasping.policy.policy.GraspingPolicy

Abstract grasp sampling policy class using Fully-Convolutional GQ-CNN network.

__init__(cfg, filters=None)
Parameters
  • cfg (dict) – Python dictionary of policy configuration parameters.

  • filters (dict) – Python dictionary of kinematic filters to apply.

action_set(state, num_actions)

Plan a set of actions.

Parameters
  • state (gqcnn.RgbdImageState) – The RGBD image state.

  • num_actions (int) – The number of actions to plan.

Returns

The planned grasps.

Return type

list of gqcnn.GraspAction

RobustGraspingPolicy

class gqcnn.RobustGraspingPolicy(config, filters=None)

Bases: gqcnn.grasping.policy.policy.GraspingPolicy

Samples a set of grasp candidates in image space, ranks the grasps by the predicted probability of success from a GQ-CNN and returns the grasp with the highest probability of success.

__init__(config, filters=None)
Parameters
  • config (dict) – Python dictionary of policy configuration parameters.

  • filters (dict) – Python dictionary of functions to apply to filter invalid grasps.

Notes

Required configuration dictionary parameters are specified in Other Parameters.

Other Parameters
  • num_grasp_samples (int) – Number of grasps to sample.

  • gripper_width (float, optional) – Width of the gripper in meters.

  • logging_dir (str, optional) – Directory in which to save the sampled grasps and input images.

select(grasps, q_value)

Selects the grasp with the highest probability of success.

Can override for alternate policies (e.g. epsilon greedy).

Parameters
  • grasps (list) – Python list of gqcnn.grasping.Grasp2D or gqcnn.grasping.SuctionPoint2D grasps to select from.

  • q_values (list) – Python list of associated q-values.

Returns

Grasp with highest probability of success .

Return type

gqcnn.grasping.Grasp2D or gqcnn.grasping.SuctionPoint2D

UniformRandomGraspingPolicy

class gqcnn.UniformRandomGraspingPolicy(config)

Bases: gqcnn.grasping.policy.policy.GraspingPolicy

Returns a grasp uniformly at random.

__init__(config)
Parameters
  • config (dict) – Python dictionary of policy configuration parameters.

  • filters (dict) – Python dictionary of functions to apply to filter invalid grasps.