From b0fc0b80a5902085dbe4ca994ed76aba1c8c6af5 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Fri, 24 May 2019 17:56:13 -0700 Subject: [PATCH 01/29] WIP Refactoring. --- docker/gpu/Dockerfile | 1 + gqcnn/__init__.py | 38 +- gqcnn/analysis/analyzer.py | 55 +- gqcnn/grasping/__init__.py | 44 +- gqcnn/grasping/actions.py | 68 +- gqcnn/grasping/constraint_fn.py | 75 +- gqcnn/grasping/grasp.py | 288 ++++---- gqcnn/grasping/grasp_quality_function.py | 656 +++++++++-------- gqcnn/grasping/image_grasp_sampler.py | 586 +++++++-------- gqcnn/grasping/policy/__init__.py | 30 +- gqcnn/grasping/policy/enums.py | 30 +- gqcnn/grasping/policy/fc_policy.py | 211 +++--- gqcnn/grasping/policy/policy.py | 871 ++++++++++++----------- gqcnn/model/__init__.py | 73 +- gqcnn/model/tf/__init__.py | 27 +- gqcnn/model/tf/fc_network_tf.py | 207 +++--- gqcnn/model/tf/network_tf.py | 794 +++++++++++---------- gqcnn/search/__init__.py | 25 +- gqcnn/search/enums.py | 34 +- gqcnn/search/resource_manager.py | 70 +- gqcnn/search/search.py | 101 +-- gqcnn/search/trial.py | 113 +-- gqcnn/search/utils.py | 70 +- gqcnn/training/__init__.py | 44 +- gqcnn/training/tf/__init__.py | 25 +- gqcnn/training/tf/trainer_tf.py | 820 ++++++++++----------- gqcnn/utils/__init__.py | 53 +- gqcnn/utils/enums.py | 118 +-- gqcnn/utils/policy_exceptions.py | 29 +- gqcnn/utils/train_stats_logger.py | 117 +-- gqcnn/utils/utils.py | 77 +- gqcnn/version.py | 23 +- requirements/cpu_requirements.txt | 6 +- requirements/gpu_requirements.txt | 6 +- setup.py | 9 +- 35 files changed, 3101 insertions(+), 2693 deletions(-) diff --git a/docker/gpu/Dockerfile b/docker/gpu/Dockerfile index d8708f29..2b8a6396 100644 --- a/docker/gpu/Dockerfile +++ b/docker/gpu/Dockerfile @@ -9,6 +9,7 @@ ARG work_dir=/root/Workspace # Install apt-get deps RUN apt-get update && apt-get install -y \ build-essential \ + python3.7 \ python-tk \ python-dev \ python-opengl \ diff --git a/gqcnn/__init__.py b/gqcnn/__init__.py index 67253b42..2303fde3 100644 --- a/gqcnn/__init__.py +++ b/gqcnn/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,11 +22,18 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from model import get_gqcnn_model, get_fc_gqcnn_model -from training import get_gqcnn_trainer -from grasping import RobustGraspingPolicy, UniformRandomGraspingPolicy, CrossEntropyRobustGraspingPolicy, RgbdImageState, FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction -from analysis import GQCNNAnalyzer -from search import GQCNNSearch -from utils import NoValidGraspsException, NoAntipodalPairsFoundException +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['get_gqcnn_model', 'get_fc_gqcnn_model', 'get_gqcnn_trainer', 'RobustGraspingPolicy', 'UniformRandomGraspingPolicy', 'CrossEntropyRobustGraspingPolicy', 'RgbdImageState','FullyConvolutionalGraspingPolicyParallelJaw', 'FullyConvolutionalGraspingPolicySuction', 'GQCNNAnalyzer', 'GQCNNSearch', 'NoValidGraspsException', 'NoAntipodalPairsFoundException'] +from .model import get_gqcnn_model, get_fc_gqcnn_model +from .training import get_gqcnn_trainer +from .grasping import (RobustGraspingPolicy, UniformRandomGraspingPolicy, + CrossEntropyRobustGraspingPolicy, RgbdImageState, + FullyConvolutionalGraspingPolicyParallelJaw, + FullyConvolutionalGraspingPolicySuction) +from .analysis import GQCNNAnalyzer +from .search import GQCNNSearch +from .utils import NoValidGraspsException, NoAntipodalPairsFoundException + +__all__ = ["get_gqcnn_model", "get_fc_gqcnn_model", "get_gqcnn_trainer", "RobustGraspingPolicy", "UniformRandomGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "RgbdImageState","FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "GQCNNAnalyzer", "GQCNNSearch", "NoValidGraspsException", "NoAntipodalPairsFoundException"] diff --git a/gqcnn/analysis/analyzer.py b/gqcnn/analysis/analyzer.py index 0ca895bb..a1bb1aaa 100644 --- a/gqcnn/analysis/analyzer.py +++ b/gqcnn/analysis/analyzer.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,42 +21,38 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Class for analyzing a GQCNN model for grasp quality prediction. +Class for analyzing a GQCNN model for grasp quality prediction. Author: Jeff Mahler """ -import cPickle as pkl +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import copy import json +import logging import os +import pickle as pkl import random import sys import time -import logging import matplotlib import matplotlib.pyplot as plt import numpy as np import scipy.misc as sm -import autolab_core.utils as utils from autolab_core import BinaryClassificationResult, Point, TensorDataset, Logger from autolab_core.constants import * +import autolab_core.utils as utils from perception import DepthImage from visualization import Visualizer2D as vis2d -from gqcnn import get_gqcnn_model -from gqcnn.grasping import Grasp2D, SuctionPoint2D -from gqcnn.utils import GripperMode, ImageMode, GeneralConstants, read_pose_data +from ..model import get_gqcnn_model +from ..grasping import Grasp2D, SuctionPoint2D +from ..utils import GripperMode, ImageMode, GeneralConstants, read_pose_data, GQCNNFilenames -PCT_POS_VAL_FILENAME = 'pct_pos_val.npy' -TRAIN_LOSS_FILENAME = 'train_losses.npy' -TRAIN_ERRORS_FILENAME = 'train_errors.npy' -VAL_ERRORS_FILENAME = 'val_errors.npy' -TRAIN_ITERS_FILENAME = 'train_eval_iters.npy' -VAL_ITERS_FILENAME = 'val_eval_iters.npy' WINDOW = 100 MAX_LOSS = 5.0 @@ -606,12 +605,12 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): # losses try: - train_errors_filename = os.path.join(model_dir, TRAIN_ERRORS_FILENAME) - val_errors_filename = os.path.join(model_dir, VAL_ERRORS_FILENAME) - train_iters_filename = os.path.join(model_dir, TRAIN_ITERS_FILENAME) - val_iters_filename = os.path.join(model_dir, VAL_ITERS_FILENAME) - pct_pos_val_filename = os.path.join(model_dir, PCT_POS_VAL_FILENAME) - train_losses_filename = os.path.join(model_dir, TRAIN_LOSS_FILENAME) + train_errors_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_ERRORS) + val_errors_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ERRORS) + train_iters_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_ITERS) + val_iters_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ITERS) + pct_pos_val_filename = os.path.join(model_dir, GQCNNFilenames.PCT_POS_VAL) + train_losses_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_LOSS) raw_train_errors = np.load(train_errors_filename) val_errors = np.load(val_errors_filename) diff --git a/gqcnn/grasping/__init__.py b/gqcnn/grasping/__init__.py index 7ef9ba85..6ad8c4af 100644 --- a/gqcnn/grasping/__init__.py +++ b/gqcnn/grasping/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,11 +22,24 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D -from grasp_quality_function import GraspQualityFunctionFactory, GQCnnQualityFunction -from image_grasp_sampler import ImageGraspSamplerFactory, AntipodalDepthImageGraspSampler -from constraint_fn import GraspConstraintFnFactory, DiscreteApproachGraspConstraintFn -from policy import RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction, UniformRandomGraspingPolicy, PriorityCompositeGraspingPolicy, RgbdImageState, GraspAction -from actions import NoAction, ParallelJawGrasp3D, SuctionGrasp3D, MultiSuctionGrasp3D +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['Grasp2D', 'SuctionPoint2D', 'MultiSuctionPoint2D', 'GraspQualityFunctionFactory', 'GQCnnQualityFunction', 'ImageGraspSamplerFactory', 'AntipodalDepthImageGraspSampler', 'RobustGraspingPolicy', 'CrossEntropyRobustGraspingPolicy', 'FullyConvolutionalGraspingPolicyParallelJaw', 'FullyConvolutionalGraspingPolicySuction', 'UniformRandomGraspingPolicy', 'RgbdImageState', 'GraspAction', 'GraspConstraintFnFactory', 'NoAction', 'ParallelJawGrasp3D', 'SuctionGrasp3D', 'MultiSuctionGrasp3D'] +from .grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D +from .grasp_quality_function import (GraspQualityFunctionFactory, + GQCnnQualityFunction) +from .image_grasp_sampler import (ImageGraspSamplerFactory, + AntipodalDepthImageGraspSampler) +from .constraint_fn import (GraspConstraintFnFactory, + DiscreteApproachGraspConstraintFn) +from .policy import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, + FullyConvolutionalGraspingPolicyParallelJaw, + FullyConvolutionalGraspingPolicySuction, + UniformRandomGraspingPolicy, + PriorityCompositeGraspingPolicy, + RgbdImageState, GraspAction) +from .actions import (NoAction, ParallelJawGrasp3D, SuctionGrasp3D, + MultiSuctionGrasp3D) + +__all__ = ["Grasp2D", "SuctionPoint2D", "MultiSuctionPoint2D", "GraspQualityFunctionFactory", "GQCnnQualityFunction", "ImageGraspSamplerFactory", "AntipodalDepthImageGraspSampler", "RobustGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "UniformRandomGraspingPolicy", "RgbdImageState", "GraspAction", "GraspConstraintFnFactory", "NoAction", "ParallelJawGrasp3D", "SuctionGrasp3D", "MultiSuctionGrasp3D"] diff --git a/gqcnn/grasping/actions.py b/gqcnn/grasping/actions.py index 3744c1fc..9578582f 100644 --- a/gqcnn/grasping/actions.py +++ b/gqcnn/grasping/actions.py @@ -1,16 +1,45 @@ +# -*- coding: utf-8 -*- """ -Action classes for representing 3D grasp actions +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Action classes for representing 3D grasp actions. Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from abc import ABCMeta, abstractmethod + +from future.utils import with_metaclass import numpy as np from autolab_core import Point, RigidTransform -from gqcnn.grasping import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D +from .grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D -class Action(object): - """ Abstract action class. +class Action(with_metaclass(ABCMeta, object)): + """Abstract action class. Attributes ---------- @@ -41,16 +70,16 @@ def metadata(self): return self._metadata class NoAction(Action): - """ Proxy for taking no action when none can be found! """ + """Proxy for taking no action when none can be found.""" pass -class GraspAction3D(Action): - """ Generic grasping with grasps specified as an end-effector pose +class GraspAction3D(with_metaclass(ABCMeta, Action)): + """Abstract grasp class with grasp specified as an end-effector pose. Attributes ---------- T_grasp_world : :obj:`RigidTransform` - pose of the grasp wrt world coordinate frame + pose of the grasp w.r.t. world coordinate frame """ def __init__(self, T_grasp_world, q_value=0.0, @@ -59,8 +88,13 @@ def __init__(self, T_grasp_world, self.T_grasp_world = T_grasp_world Action.__init__(self, q_value, id, metadata) + @abstractmethod + def project(self, camera_intr, + T_camera_world): + pass + class ParallelJawGrasp3D(GraspAction3D): - """ Grasping with a parallel-jaw gripper. + """Grasping with a parallel-jaw gripper. Attributes ---------- @@ -70,13 +104,13 @@ class ParallelJawGrasp3D(GraspAction3D): def project(self, camera_intr, T_camera_world, gripper_width=0.05): - # compute pose of grasp in camera frame + # Compute pose of grasp in camera frame. T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world y_axis_camera = T_grasp_camera.y_axis[:2] if np.linalg.norm(y_axis_camera) > 0: y_axis_camera = y_axis_camera / np.linalg.norm(y_axis_camera) - # compute grasp axis rotation in image space + # Compute grasp axis rotation in image space. rot_grasp_camera = np.arccos(y_axis_camera[0]) if y_axis_camera[1] < 0: rot_grasp_camera = -rot_grasp_camera @@ -85,7 +119,7 @@ def project(self, camera_intr, while rot_grasp_camera > 2 * np.pi: rot_grasp_camera -= 2 * np.pi - # compute grasp center in image space + # Compute grasp center in image space. t_grasp_camera = T_grasp_camera.translation p_grasp_camera = Point(t_grasp_camera, frame=camera_intr.frame) @@ -98,7 +132,7 @@ def project(self, camera_intr, camera_intr=camera_intr) class SuctionGrasp3D(GraspAction3D): - """ Grasping with a suction-based gripper. + """Grasping with a suction-based gripper. Attributes ---------- @@ -107,11 +141,11 @@ class SuctionGrasp3D(GraspAction3D): """ def project(self, camera_intr, T_camera_world): - # compute pose of grasp in camera frame + # Compute pose of grasp in camera frame. T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world x_axis_camera = T_grasp_camera.x_axis - # compute grasp center in image space + # Compute grasp center in image space. t_grasp_camera = T_grasp_camera.translation p_grasp_camera = Point(t_grasp_camera, frame=camera_intr.frame) u_grasp_camera = camera_intr.project(p_grasp_camera) @@ -122,7 +156,7 @@ def project(self, camera_intr, camera_intr=camera_intr) class MultiSuctionGrasp3D(GraspAction3D): - """ Grasping with a multi-cup suction-based gripper. + """Grasping with a multi-cup suction-based gripper. Attributes ---------- @@ -131,7 +165,7 @@ class MultiSuctionGrasp3D(GraspAction3D): """ def project(self, camera_intr, T_camera_world): - # compute pose of grasp in camera frame + # Compute pose of grasp in camera frame. T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world return MultiSuctionPoint2D(T_grasp_camera, camera_intr=camera_intr) diff --git a/gqcnn/grasping/constraint_fn.py b/gqcnn/grasping/constraint_fn.py index 5613450d..a1e4df46 100644 --- a/gqcnn/grasping/constraint_fn.py +++ b/gqcnn/grasping/constraint_fn.py @@ -1,24 +1,48 @@ +# -*- coding: utf-8 -*- """ -Constraint functions for grasp sampling +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Constraint functions for grasp sampling. Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from abc import ABCMeta, abstractmethod +from future.utils import with_metaclass import numpy as np -class GraspConstraintFn(object): - """ - Abstract constraint functions for grasp sampling. - """ - __metaclass__ = ABCMeta +class GraspConstraintFn(with_metaclass(ABCMeta, object)): + """Abstract constraint functions for grasp sampling.""" def __init__(self, config): - # set params + # Set params. self._config = config def __call__(self, grasp): - """ - Evaluates whether or not a grasp is valid. + """Evaluates whether or not a grasp is valid. Parameters ---------- @@ -34,8 +58,7 @@ def __call__(self, grasp): @abstractmethod def satisfies_constraints(self, grasp): - """ - Evaluates whether or not a grasp is valid. + """Evaluates whether or not a grasp is valid. Parameters ---------- @@ -50,23 +73,21 @@ def satisfies_constraints(self, grasp): pass class DiscreteApproachGraspConstraintFn(GraspConstraintFn): - """ - Constrains the grasp approach direction into a discrete set of - angles from the world z direction. + """Constrains the grasp approach direction into a discrete set of + angles from the world z direction. """ def __init__(self, config): - # init superclass + # Init superclass. GraspConstraintFn.__init__(self, config) - self._max_approach_angle = self._config['max_approach_angle'] - self._angular_tolerance = self._config['angular_tolerance'] - self._angular_step = self._config['angular_step'] - self._T_camera_world = self._config['camera_pose'] + self._max_approach_angle = self._config["max_approach_angle"] + self._angular_tolerance = self._config["angular_tolerance"] + self._angular_step = self._config["angular_step"] + self._T_camera_world = self._config["camera_pose"] def satisfies_constraints(self, grasp): - """ - Evaluates whether or not a grasp is valid by evaluating the - angle between the approach axis and the world z direction. + """Evaluates whether or not a grasp is valid by evaluating the + angle between the approach axis and the world z direction. Parameters ---------- @@ -78,11 +99,11 @@ def satisfies_constraints(self, grasp): bool True if the grasp satisfies constraints, False otherwise """ - # find grasp angle in world coordinates + # Find grasp angle in world coordinates. axis_world = self._T_camera_world.rotation.dot(grasp.approach_axis) angle = np.arccos(-axis_world[2]) - # check closest available angle + # Check closest available angle. available_angles = np.array([0.0]) if self._angular_step > 0: available_angles = np.arange(start=0.0, @@ -98,9 +119,9 @@ def satisfies_constraints(self, grasp): class GraspConstraintFnFactory(object): @staticmethod def constraint_fn(fn_type, config): - if fn_type == 'none': + if fn_type == "none": return None - elif fn_type == 'discrete_approach_angle': + elif fn_type == "discrete_approach_angle": return DiscreteApproachGraspConstraintFn(config) else: - raise ValueError('Grasp constraint function type %s not supported!' %(fn_type)) + raise ValueError("Grasp constraint function type {} not supported!".format(fn_type)) diff --git a/gqcnn/grasping/grasp.py b/gqcnn/grasping/grasp.py index bdbacf49..b13fa77d 100644 --- a/gqcnn/grasping/grasp.py +++ b/gqcnn/grasping/grasp.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,36 +21,38 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Classes to encapsulate parallel-jaw grasps in image space + +Classes to encapsulate parallel-jaw grasps in image space. Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import numpy as np from autolab_core import Point, RigidTransform from perception import CameraIntrinsics class Grasp2D(object): - """ - Parallel-jaw grasp in image space. + """Parallel-jaw grasp in image space. Attributes ---------- center : :obj:`autolab_core.Point` - point in image space + Point in image space. angle : float - grasp axis angle with the camera x-axis + Grasp axis angle with the camera x-axis. depth : float - depth of the grasp center in 3D space + Depth of the grasp center in 3D space. width : float - distance between the jaws in meters + Distance between the jaws in meters. camera_intr : :obj:`perception.CameraIntrinsics` - frame of reference for camera that the grasp corresponds to + Frame of reference for camera that the grasp corresponds to. contact_points : list of :obj:`numpy.ndarray` - pair of contact points in image space + Pair of contact points in image space. contact_normals : list of :obj:`numpy.ndarray` - pair of contact normals in image space + Pair of contact normals in image space. """ def __init__(self, center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, contact_points=None, contact_normals=None): @@ -55,15 +60,15 @@ def __init__(self, center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, self.angle = angle self.depth = depth self.width = width - # if camera_intr is none use default primesense camera intrinsics + # If `camera_intr` is none use default primesense camera intrinsics. if not camera_intr: - self.camera_intr = CameraIntrinsics('primesense_overhead', fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) + self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr self.contact_points = contact_points self.contact_normals = contact_normals - frame = 'image' + frame = "image" if camera_intr is not None: frame = camera_intr.frame if isinstance(center, np.ndarray): @@ -71,7 +76,7 @@ def __init__(self, center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, @property def axis(self): - """ Returns the grasp axis. """ + """Returns the grasp axis.""" return np.array([np.cos(self.angle), np.sin(self.angle)]) @property @@ -80,67 +85,70 @@ def approach_axis(self): @property def approach_angle(self): - """ The angle between the grasp approach axis and camera optical axis. """ + """The angle between the grasp approach axis and camera optical axis. + """ return 0.0 @property def frame(self): - """ The name of the frame of reference for the grasp. """ + """The name of the frame of reference for the grasp.""" if self.camera_intr is None: - raise ValueError('Must specify camera intrinsics') + raise ValueError("Must specify camera intrinsics") return self.camera_intr.frame @property def width_px(self): - """ Returns the width in pixels. """ + """Returns the width in pixels.""" if self.camera_intr is None: - raise ValueError('Must specify camera intrinsics to compute gripper width in 3D space') - # form the jaw locations in 3D space at the given depth + raise ValueError("Must specify camera intrinsics to compute gripper width in 3D space") + # Form the jaw locations in 3D space at the given depth. p1 = Point(np.array([0, 0, self.depth]), frame=self.frame) p2 = Point(np.array([self.width, 0, self.depth]), frame=self.frame) - # project into pixel space + # Project into pixel space. u1 = self.camera_intr.project(p1) u2 = self.camera_intr.project(p2) return np.linalg.norm(u1.data - u2.data) @property def endpoints(self): - """ Returns the grasp endpoints """ - p1 = self.center.data - (float(self.width_px) / 2) * self.axis - p2 = self.center.data + (float(self.width_px) / 2) * self.axis + """Returns the grasp endpoints.""" + p1 = self.center.data - (self.width_px / 2) * self.axis + p2 = self.center.data + (self.width_px / 2) * self.axis return p1, p2 @property def feature_vec(self): - """ Returns the feature vector for the grasp. - v = [p1, p2, depth] - where p1 and p2 are the jaw locations in image space + """Returns the feature vector for the grasp. + + `v = [p1, p2, depth]` where `p1` and `p2` are the jaw locations in + image space. """ p1, p2 = self.endpoints return np.r_[p1, p2, self.depth] @staticmethod def from_feature_vec(v, width=0.0, camera_intr=None): - """ Creates a Grasp2D obj from a feature vector and additional parameters. + """Creates a `Grasp2D` instance from a feature vector and additional + parameters. Parameters ---------- v : :obj:`numpy.ndarray` - feature vector, see Grasp2D.feature_vec + Feature vector, see `Grasp2D.feature_vec`. width : float - grasp opening width, in meters + Grasp opening width, in meters. camera_intr : :obj:`perception.CameraIntrinsics` - frame of reference for camera that the grasp corresponds to + Frame of reference for camera that the grasp corresponds to. """ - # read feature vec + # Read feature vec. p1 = v[:2] p2 = v[2:4] depth = v[4] - # compute center and angle - center_px = (p1 + p2) / 2 + # Compute center and angle. + center_px = (p1 + p2) // 2 center = Point(center_px, camera_intr.frame) axis = p2 - p1 if np.linalg.norm(axis) > 0: @@ -152,95 +160,96 @@ def from_feature_vec(v, width=0.0, camera_intr=None): return Grasp2D(center, angle, depth, width=width, camera_intr=camera_intr) def pose(self, grasp_approach_dir=None): - """ Computes the 3D pose of the grasp relative to the camera. + """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 : :obj:`numpy.ndarray` - approach direction for the grasp in camera basis (e.g. opposite to table normal) + Approach direction for the grasp in camera basis (e.g. opposite to + table normal). Returns ------- - :obj:`autolab_core.RigidTransform` - the transformation from the grasp to the camera frame of reference + :obj:`autolab_core.RigidTransform`: The transformation from the grasp + to the camera frame of reference. """ - # check intrinsics + # Check intrinsics. if self.camera_intr is None: - raise ValueError('Must specify camera intrinsics to compute 3D grasp pose') + raise ValueError("Must specify camera intrinsics to compute 3D grasp pose") - # compute 3D grasp center in camera basis + # Compute 3D grasp center in camera basis. grasp_center_im = self.center.data center_px_im = Point(grasp_center_im, frame=self.camera_intr.frame) grasp_center_camera = self.camera_intr.deproject_pixel(self.depth, center_px_im) grasp_center_camera = grasp_center_camera.data - # compute 3D grasp axis in camera basis + # Compute 3D grasp axis in camera basis. grasp_axis_im = self.axis grasp_axis_im = grasp_axis_im / np.linalg.norm(grasp_axis_im) grasp_axis_camera = np.array([grasp_axis_im[0], grasp_axis_im[1], 0]) grasp_axis_camera = grasp_axis_camera / np.linalg.norm(grasp_axis_camera) - # convert to 3D pose + # Convert to 3D pose. grasp_rot_camera, _, _ = np.linalg.svd(grasp_axis_camera.reshape(3,1)) grasp_x_camera = grasp_approach_dir if grasp_approach_dir is None: - grasp_x_camera = np.array([0,0,1]) # aligned with camera Z axis + grasp_x_camera = np.array([0,0,1]) # Align with camera Z axis. grasp_y_camera = grasp_axis_camera grasp_z_camera = np.cross(grasp_x_camera, grasp_y_camera) grasp_z_camera = grasp_z_camera / np.linalg.norm(grasp_z_camera) grasp_y_camera = np.cross(grasp_z_camera, grasp_x_camera) grasp_rot_camera = np.array([grasp_x_camera, grasp_y_camera, grasp_z_camera]).T - if np.linalg.det(grasp_rot_camera) < 0: # fix possible reflections due to SVD + if np.linalg.det(grasp_rot_camera) < 0: # Fix possible reflections due to SVD. grasp_rot_camera[:,0] = -grasp_rot_camera[:,0] T_grasp_camera = RigidTransform(rotation=grasp_rot_camera, translation=grasp_center_camera, - from_frame='grasp', + from_frame="grasp", to_frame=self.camera_intr.frame) return T_grasp_camera @staticmethod def image_dist(g1, g2, alpha=1.0): - """ Computes the distance between grasps in image space. - Euclidean distance with alpha weighting of angles + """Computes the distance between grasps in image space. + + Uses Euclidean distance with alpha weighting of angles Parameters ---------- g1 : :obj:`Grasp2D` - first grasp + First grasp. g2 : :obj:`Grasp2D` - second grasp + Second grasp. alpha : float - weight of angle distance (rad to meters) + Weight of angle distance (rad to meters). Returns ------- - float - distance between grasps + float: Distance between grasps. """ - # point to point distances + # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) - # axis distances + # Axis distances. dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0) axis_dist = np.arccos(dot) return point_dist + alpha * axis_dist class SuctionPoint2D(object): - """ - Suction grasp in image space. + """Suction grasp in image space. Attributes ---------- center : :obj:`autolab_core.Point` - point in image space + Point in image space. axis : :obj:`numpy.ndarray` - normalized 3-vector representing the direction of the suction tip + Dormalized 3-vector representing the direction of the suction tip. depth : float - depth of the suction point in 3D space + Depth of the suction point in 3D space. camera_intr : :obj:`perception.CameraIntrinsics` - frame of reference for camera that the suction point corresponds to + Frame of reference for camera that the suction point corresponds to. """ def __init__(self, center, axis=None, depth=1.0, camera_intr=None): if axis is None: @@ -249,7 +258,7 @@ def __init__(self, center, axis=None, depth=1.0, camera_intr=None): self.center = center self.axis = axis - frame = 'image' + frame = "image" if camera_intr is not None: frame = camera_intr.frame if isinstance(center, np.ndarray): @@ -257,25 +266,25 @@ def __init__(self, center, axis=None, depth=1.0, camera_intr=None): if isinstance(axis, list): self.axis = np.array(axis) if np.abs(np.linalg.norm(self.axis) - 1.0) > 1e-3: - raise ValueError('Illegal axis. Must be norm 1.') + raise ValueError("Illegal axis. Must be norm 1.") self.depth = depth - # if camera_intr is none use default primesense camera intrinsics + # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: - self.camera_intr = CameraIntrinsics('primesense_overhead', fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) + self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr @property def frame(self): - """ The name of the frame of reference for the grasp. """ + """The name of the frame of reference for the grasp.""" if self.camera_intr is None: - raise ValueError('Must specify camera intrinsics') + raise ValueError("Must specify camera intrinsics") return self.camera_intr.frame @property def angle(self): - """ The angle that the grasp pivot axis makes in image space. """ + """The angle that the grasp pivot axis makes in image space.""" rotation_axis = np.cross(self.axis, np.array([0,0,1])) rotation_axis_image = np.array([rotation_axis[0], rotation_axis[1]]) angle = 0 @@ -288,7 +297,8 @@ def angle(self): @property def approach_angle(self): - """ The angle between the grasp approach axis and camera optical axis. """ + """The angle between the grasp approach axis and camera optical axis. + """ dot = max(min(self.axis.dot(np.array([0,0,1])), 1.0), -1.0) return np.arccos(dot) @@ -298,27 +308,31 @@ def approach_axis(self): @property def feature_vec(self): - """ Returns the feature vector for the suction point. - v = [center, axis, depth] + """Returns the feature vector for the suction point. + + Note + ---- + `v = [center, axis, depth]` """ return self.center.data @staticmethod def from_feature_vec(v, camera_intr=None, depth=None, axis=None): - """ Creates a SuctionPoint2D obj from a feature vector and additional parameters. + """Creates a `SuctionPoint2D` instance from a feature vector and + additional parameters. Parameters ---------- v : :obj:`numpy.ndarray` - feature vector, see Grasp2D.feature_vec + Feature vector, see `Grasp2D.feature_vec`. camera_intr : :obj:`perception.CameraIntrinsics` - frame of reference for camera that the grasp corresponds to + Frame of reference for camera that the grasp corresponds to. depth : float - hard-set the depth for the suction grasp + Hard-set the depth for the suction grasp. axis : :obj:`numpy.ndarray` - normalized 3-vector specifying the approach direction + Normalized 3-vector specifying the approach direction. """ - # read feature vec + # Read feature vec. center_px = v[:2] grasp_axis = np.array([0,0,-1]) @@ -334,7 +348,7 @@ def from_feature_vec(v, camera_intr=None, depth=None, axis=None): elif depth is not None: grasp_depth = depth - # compute center and angle + # Compute center and angle. center = Point(center_px, camera_intr.frame) return SuctionPoint2D(center, grasp_axis, @@ -342,27 +356,27 @@ def from_feature_vec(v, camera_intr=None, depth=None, axis=None): camera_intr=camera_intr) def pose(self): - """ Computes the 3D pose of the grasp relative to the camera. + """Computes the 3D pose of the grasp relative to the camera. Returns ------- :obj:`autolab_core.RigidTransform` - the transformation from the grasp to the camera frame of reference + The transformation from the grasp to the camera frame of reference. """ - # check intrinsics + # Check intrinsics. if self.camera_intr is None: - raise ValueError('Must specify camera intrinsics to compute 3D grasp pose') + raise ValueError("Must specify camera intrinsics to compute 3D grasp pose") - # compute 3D grasp center in camera basis + # Compute 3D grasp center in camera basis. suction_center_im = self.center.data center_px_im = Point(suction_center_im, frame=self.camera_intr.frame) suction_center_camera = self.camera_intr.deproject_pixel(self.depth, center_px_im) suction_center_camera = suction_center_camera.data - # compute 3D grasp axis in camera basis + # Compute 3D grasp axis in camera basis. suction_axis_camera = self.axis - # convert to 3D pose + # Convert to 3D pose. suction_x_camera = suction_axis_camera suction_z_camera = np.array([-suction_x_camera[1], suction_x_camera[0], 0]) if np.linalg.norm(suction_z_camera) < 1e-12: @@ -373,59 +387,60 @@ def pose(self): T_suction_camera = RigidTransform(rotation=suction_rot_camera, translation=suction_center_camera, - from_frame='grasp', + from_frame="grasp", to_frame=self.camera_intr.frame) return T_suction_camera @staticmethod def image_dist(g1, g2, alpha=1.0): - """ Computes the distance between grasps in image space. - Euclidean distance with alpha weighting of angles + """Computes the distance between grasps in image space. + + Uses Euclidean distance with alpha weighting of angles. Parameters ---------- g1 : :obj:`SuctionPoint2D` - first suction point + First suction point. g2 : :obj:`SuctionPoint2D` - second suction point + Second suction point. alpha : float - weight of angle distance (rad to meters) + Weight of angle distance (rad to meters). Returns ------- - float - distance between grasps + float: Distance between grasps. """ - # point to point distances + # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) - # axis distances + # Axis distances. dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0) axis_dist = np.arccos(dot) return point_dist + alpha * axis_dist class MultiSuctionPoint2D(object): - """ - Multi-Cup Suction grasp in image space. Equivalent to projecting a 6D pose to image space. + """Multi-Cup Suction grasp in image space. + + Equivalent to projecting a 6D pose to image space. Attributes ---------- pose : :obj:`autolab_core.RigidTransform` - pose in 3D camera space + Pose in 3D camera space. camera_intr : :obj:`perception.CameraIntrinsics` - frame of reference for camera that the suction point corresponds to + Frame of reference for camera that the suction point corresponds to. """ def __init__(self, pose, camera_intr=None): self._pose = pose - frame = 'image' + frame = "image" if camera_intr is not None: frame = camera_intr.frame - # if camera_intr is none use default primesense camera intrinsics + # If camera_intr is none use default primesense camera intrinsics. if not camera_intr: - self.camera_intr = CameraIntrinsics('primesense_overhead', fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) + self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) else: self.camera_intr = camera_intr @@ -434,9 +449,9 @@ def pose(self): @property def frame(self): - """ The name of the frame of reference for the grasp. """ + """The name of the frame of reference for the grasp.""" if self.camera_intr is None: - raise ValueError('Must specify camera intrinsics') + raise ValueError("Must specify camera intrinsics") return self.camera_intr.frame @property @@ -455,7 +470,8 @@ def approach_axis(self): @property def approach_angle(self): - """ The angle between the grasp approach axis and camera optical axis. """ + """The angle between the grasp approach axis and camera optical axis. + """ dot = max(min(self.axis.dot(np.array([0,0,1])), 1.0), -1.0) return np.arccos(dot) @@ -489,32 +505,36 @@ def orientation(self): @property def feature_vec(self): - """ Returns the feature vector for the suction point. - v = [center, axis, depth] + """Returns the feature vector for the suction point. + + Note + ---- + `v = [center, axis, depth]` """ return np.r_[self.center.data, np.cos(self.orientation), np.sin(self.orientation)] @staticmethod def from_feature_vec(v, camera_intr=None, angle=None, depth=None, axis=None): - """ Creates a SuctionPoint2D obj from a feature vector and additional parameters. + """Creates a `SuctionPoint2D` instance from a feature vector and + additional parameters. Parameters ---------- v : :obj:`numpy.ndarray` - feature vector, see Grasp2D.feature_vec + Feature vector, see `Grasp2D.feature_vec`. camera_intr : :obj:`perception.CameraIntrinsics` - frame of reference for camera that the grasp corresponds to + Frame of reference for camera that the grasp corresponds to. depth : float - hard-set the depth for the suction grasp + Hard-set the depth for the suction grasp. axis : :obj:`numpy.ndarray` - normalized 3-vector specifying the approach direction + Normalized 3-vector specifying the approach direction. """ - # read feature vec + # Read feature vec. center_px = v[:2] grasp_angle = 0 if v.shape > 2 and angle is None: - #grasp_angle = v[2] + # grasp_angle = v[2] grasp_vec = v[2:] grasp_vec = grasp_vec / np.linalg.norm(grasp_vec) grasp_angle = np.arctan2(grasp_vec[1], grasp_vec[0]) @@ -541,35 +561,35 @@ def from_feature_vec(v, camera_intr=None, angle=None, depth=None, axis=None): t = camera_intr.deproject_pixel(grasp_depth, Point(center_px, frame=camera_intr.frame)).data T = RigidTransform(rotation=R, translation=t, - from_frame='grasp', + from_frame="grasp", to_frame=camera_intr.frame) - # compute center and angle + # Compute center and angle. return MultiSuctionPoint2D(T, camera_intr=camera_intr) @staticmethod def image_dist(g1, g2, alpha=1.0): - """ Computes the distance between grasps in image space. - Euclidean distance with alpha weighting of angles + """Computes the distance between grasps in image space. + + Uses Euclidean distance with alpha weighting of angles. Parameters ---------- g1 : :obj:`SuctionPoint2D` - first suction point + First suction point. g2 : :obj:`SuctionPoint2D` - second suction point + Second suction point. alpha : float - weight of angle distance (rad to meters) + Weight of angle distance (rad to meters). Returns ------- - float - distance between grasps + float: Distance between grasps. """ - # point to point distances + # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) - # axis distances + # Axis distances. dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0) axis_dist = np.arccos(dot) diff --git a/gqcnn/grasping/grasp_quality_function.py b/gqcnn/grasping/grasp_quality_function.py index bc3cb34d..11b41f19 100644 --- a/gqcnn/grasping/grasp_quality_function.py +++ b/gqcnn/grasping/grasp_quality_function.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,96 +21,100 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Grasp quality functions: suction quality function and parallel jaw grasping +quality fuction. +Authors: Jason Liu and Jeff Mahler """ -""" -Grasp quality functions: suction quality function and parallel jaw grasping quality fuction. -Author: Jason Liu and Jeff Mahler -""" +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from abc import ABCMeta, abstractmethod from time import time -import scipy.ndimage.filters as snf import cv2 -import numpy as np +from future.utils import with_metaclass import matplotlib.pyplot as plt +import numpy as np +import scipy.ndimage.filters as snf import autolab_core.utils as utils from autolab_core import Point, PointCloud, RigidTransform, Logger -from perception import RgbdImage, CameraIntrinsics, PointCloudImage, ColorImage, BinaryImage, DepthImage, GrayscaleImage -from gqcnn import get_gqcnn_model, get_fc_gqcnn_model -from gqcnn.grasping import Grasp2D, SuctionPoint2D -from gqcnn.utils import GripperMode +from perception import (RgbdImage, CameraIntrinsics, PointCloudImage, + ColorImage, BinaryImage, DepthImage, GrayscaleImage) -# constant for display -FIGSIZE = 16 +from ..model import get_gqcnn_model, get_fc_gqcnn_model +from .grasp import Grasp2D, SuctionPoint2D +from ..utils import GeneralConstants, GripperMode -class GraspQualityFunction(object): - """Abstract grasp quality class. """ - __metaclass__ = ABCMeta +class GraspQualityFunction(with_metaclass(ABCMeta, object)): + """Abstract grasp quality class.""" def __init__(self): - # set up logger + # Set up logger. self._logger = Logger.get_logger(self.__class__.__name__) def __call__(self, state, actions, params=None): - """ Evaluates grasp quality for a set of actions given a state. """ + """Evaluates grasp quality for a set of actions given a state.""" return self.quality(state, actions, params) @abstractmethod def quality(self, state, actions, params=None): - """ Evaluates grasp quality for a set of actions given a state. + """Evaluates grasp quality for a set of actions given a state. Parameters ---------- state : :obj:`object` - state of the world e.g. image + State of the world e.g. image. actions : :obj:`list` - list of actions to evaluate e.g. parallel-jaw or suction grasps + List of actions to evaluate e.g. parallel-jaw or suction grasps. params : :obj:`dict` - optional parameters for the evaluation + Optional parameters for the evaluation. Returns ------- - :obj:`numpy.ndarray` - vector containing the real-valued grasp quality for each candidate + :obj:`numpy.ndarray`: Vector containing the real-valued grasp quality + for each candidate. """ pass class ZeroGraspQualityFunction(object): - """ Null function. """ + """Null function.""" def quality(self, state, actions, params=None): - """ Returns zero for all grasps + """Returns zero for all grasps. Parameters ---------- state : :obj:`object` - state of the world e.g. image + State of the world e.g. image. actions : :obj:`list` - list of actions to evaluate e.g. parallel-jaw or suction grasps + List of actions to evaluate e.g. parallel-jaw or suction grasps. params : :obj:`dict` - optional parameters for the evaluation + Optional parameters for the evaluation. Returns ------- - :obj:`numpy.ndarray` - vector containing the real-valued grasp quality for each candidate + :obj:`numpy.ndarray`: Vector containing the real-valued grasp quality + for each candidate. """ return 0.0 class ParallelJawQualityFunction(GraspQualityFunction): - """Abstract wrapper class for parallel jaw quality functions (only image based metrics for now). """ + """Abstract wrapper class for parallel jaw quality functions (only image + based metrics for now).""" def __init__(self, config): - """Create a suction quality function. """ GraspQualityFunction.__init__(self) - # read parameters - self._friction_coef = config['friction_coef'] + # Read parameters. + self._friction_coef = config["friction_coef"] self._max_friction_cone_angle = np.arctan(self._friction_coef) def friction_cone_angle(self, action): - """ Compute the angle between the axis and the boundaries of the friction cone. """ + """Compute the angle between the axis and the boundaries of the + friction cone.""" if action.contact_points is None or action.contact_normals is None: - raise ValueError('Cannot compute friction cone angle without precomputed contact points and normals') + raise ValueError("Cannot compute friction cone angle without precomputed contact points and normals") dot_prod1 = min(max(action.contact_normals[0].dot(-action.axis), -1.0), 1.0) angle1 = np.arccos(dot_prod1) dot_prod2 = min(max(action.contact_normals[1].dot(action.axis), -1.0), 1.0) @@ -115,43 +122,46 @@ def friction_cone_angle(self, action): return max(angle1, angle2) def force_closure(self, action): - """ Determine if the grasp is in force closure. """ + """Determine if the grasp is in force closure.""" return (self.friction_cone_angle(action) < self._max_friction_cone_angle) class ComForceClosureParallelJawQualityFunction(ParallelJawQualityFunction): - """ Measures the distance to the estimated center of mass for antipodal parallel-jaw grasps. """ + """Measures the distance to the estimated center of mass for antipodal + parallel-jaw grasps.""" def __init__(self, config): - """Create a best-fit planarity suction metric. """ - self._antipodality_pctile = config['antipodality_pctile'] + """Create a best-fit planarity suction metric.""" + self._antipodality_pctile = config["antipodality_pctile"] ParallelJawQualityFunction.__init__(self, config) def quality(self, state, actions, params=None): - """Given a parallel-jaw grasp, compute the distance to the center of mass of the grasped object + """Given a parallel-jaw grasp, compute the distance to the center of + mass of the grasped object. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`Grasp2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ - # compute antipodality + # Compute antipodality. antipodality_q = [ParallelJawQualityFunction.friction_cone_angle(self, action) for action in actions] - # compute object centroid + # Compute object centroid. object_com = state.rgbd_im.center if state.segmask is not None: nonzero_px = state.segmask.nonzero_pixels() object_com = np.mean(nonzero_px, axis=0) - # compute negative SSE from the best fit plane for each grasp + # Compute negative SSE from the best fit plane for each grasp. antipodality_thresh = abs(np.percentile(antipodality_q, 100-self._antipodality_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) @@ -180,34 +190,35 @@ def quality(self, state, actions, params=None): return np.array(qualities) class SuctionQualityFunction(GraspQualityFunction): - """Abstract wrapper class for suction quality functions (only image based metrics for now). """ + """Abstract wrapper class for suction quality functions (only image based + metrics for now).""" def __init__(self, config): - """Create a suction quality function. """ GraspQualityFunction.__init(self) - # read parameters - self._window_size = config['window_size'] - self._sample_rate = config['sample_rate'] + # Read parameters. + self._window_size = config["window_size"] + self._sample_rate = config["sample_rate"] def _points_in_window(self, point_cloud_image, action, segmask=None): - """Retrieve all points on the object in a box of size self._window_size. """ - # read indices + """Retrieve all points on the object in a box of size + `self._window_size`.""" + # Read indices. im_shape = point_cloud_image.shape - i_start = int(max(action.center.y-self._window_size/2, 0)) - j_start = int(max(action.center.x-self._window_size/2, 0)) + i_start = int(max(action.center.y-self._window_size//2, 0)) # TODO: Confirm div. + j_start = int(max(action.center.x-self._window_size//2, 0)) i_end = int(min(i_start+self._window_size, im_shape[0])) j_end = int(min(j_start+self._window_size, im_shape[1])) step = int(1 / self._sample_rate) - # read 3D points in the window + # Read 3D points in the window. points = point_cloud_image[i_start:i_end:step, j_start:j_end:step] stacked_points = points.reshape(points.shape[0]*points.shape[1], -1) - # form the matrices for plane-fitting + # Form the matrices for plane-fitting. return stacked_points def _points_to_matrices(self, points): - """ Convert a set of 3D points to an A and b matrix for regression. """ + """Convert a set of 3D points to an A and b matrix for regression.""" A = points[:, [0, 1]] ones = np.ones((A.shape[0], 1)) A = np.hstack((A, ones)) @@ -215,60 +226,61 @@ def _points_to_matrices(self, points): return A, b def _best_fit_plane(self, A, b): - """Find a best-fit plane of points. """ + """Find a best-fit plane of points.""" try: w, _, _, _ = np.linalg.lstsq(A, b) except np.linalg.LinAlgError: - self._logger.warning('Could not find a best-fit plane!') + self._logger.warning("Could not find a best-fit plane!") raise return w def _sum_of_squared_residuals(self, w, A, z): - """ Returns the sum of squared residuals from the plane. """ + """Returns the sum of squared residuals from the plane.""" return (1.0 / A.shape[0]) * np.square(np.linalg.norm(np.dot(A, w) - z)) class BestFitPlanaritySuctionQualityFunction(SuctionQualityFunction): - """A best-fit planarity suction metric. """ + """A best-fit planarity suction metric.""" def __init__(self, config): - """Create a best-fit planarity suction metric. """ SuctionQualityFunction.__init__(self, config) def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ qualities = [] - # deproject points + # Deproject points. point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) - # compute negative SSE from the best fit plane for each grasp + # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError('This function can only be used to evaluate suction quality') + raise ValueError("This function can only be used to evaluate suction quality") points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. A, b = self._points_to_matrices(points) w = self._best_fit_plane(A, b) # vector w w/ a bias term represents a best-fit plane. - if params is not None and params['vis']['plane']: + if params is not None and params["vis"]["plane"]: from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d - mid_i = A.shape[0] / 2 + mid_i = A.shape[0] // 2 pred_z = A.dot(w) p0 = np.array([A[mid_i,0], A[mid_i,1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) @@ -279,8 +291,8 @@ def quality(self, state, actions, params=None): R = np.array([tx, ty, n]).T T_table_world = RigidTransform(rotation=R, translation=p0, - from_frame='patch', - to_frame='world') + from_frame="patch", + to_frame="world") vis3d.figure() vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0,0,1)) @@ -290,65 +302,67 @@ def quality(self, state, actions, params=None): vis2d.figure() vis2d.imshow(state.rgbd_im.depth) - vis2d.scatter(action.center.x, action.center.y, s=50, c='b') + vis2d.scatter(action.center.x, action.center.y, s=50, c="b") vis2d.show() - quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) # evaluate how well best-fit plane describles all points in window. + quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) # Evaluate how well best-fit plane describles all points in window. qualities.append(quality) return np.array(qualities) class ApproachPlanaritySuctionQualityFunction(SuctionQualityFunction): - """A approach planarity suction metric. """ + """A approach planarity suction metric.""" def __init__(self, config): - """Create approach planarity suction metric. """ + """Create approach planarity suction metric.""" SuctionQualityFunction.__init__(self, config) def _action_to_plane(self, point_cloud_image, action): - """Convert a plane from point-normal form to general form. """ + """Convert a plane from point-normal form to general form.""" x = int(action.center.x) y = int(action.center.y) p_0 = point_cloud_image[y, x] n = -action.axis - w = np.array([-n[0]/n[2], -n[1]/n[2], np.dot(n,p_0)/n[2]]) + w = np.array([-n[0]/n[2], -n[1]/n[2], np.dot(n,p_0)/n[2]]) # TODO: Confirm divs. return w def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ qualities = [] - # deproject points + # Deproject points. point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) - # compute negative SSE from the best fit plane for each grasp + # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError('This function can only be used to evaluate suction quality') + raise ValueError("This function can only be used to evaluate suction quality") points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. A, b = self._points_to_matrices(points) w = self._action_to_plane(point_cloud_image, action) # vector w w/ a bias term represents a best-fit plane. - if params is not None and params['vis']['plane']: + if params is not None and params["vis"]["plane"]: from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d - mid_i = A.shape[0] / 2 + mid_i = A.shape[0] // 2 pred_z = A.dot(w) p0 = np.array([A[mid_i,0], A[mid_i,1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) @@ -363,8 +377,8 @@ def quality(self, state, actions, params=None): T_table_world = RigidTransform(rotation=R, translation=p0, - from_frame='patch', - to_frame='world') + from_frame="patch", + to_frame="world") vis3d.figure() vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0,0,1)) @@ -376,102 +390,105 @@ def quality(self, state, actions, params=None): vis2d.figure() vis2d.imshow(state.rgbd_im.depth) - vis2d.scatter(action.center.x, action.center.y, s=50, c='b') + vis2d.scatter(action.center.x, action.center.y, s=50, c="b") vis2d.show() - quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) # evaluate how well best-fit plane describles all points in window. + quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) # Evaluate how well best-fit plane describles all points in window. qualities.append(quality) return np.array(qualities) class DiscApproachPlanaritySuctionQualityFunction(SuctionQualityFunction): - """A approach planarity suction metric using a disc-shaped window. """ + """A approach planarity suction metric using a disc-shaped window.""" def __init__(self, config): - """Create approach planarity suction metric. """ - self._radius = config['radius'] + """Create approach planarity suction metric.""" + self._radius = config["radius"] SuctionQualityFunction.__init__(self, config) def _action_to_plane(self, point_cloud_image, action): - """Convert a plane from point-normal form to general form. """ + """Convert a plane from point-normal form to general form.""" x = int(action.center.x) y = int(action.center.y) p_0 = point_cloud_image[y, x] n = -action.axis - w = np.array([-n[0]/n[2], -n[1]/n[2], np.dot(n,p_0)/n[2]]) + w = np.array([-n[0]/n[2], -n[1]/n[2], np.dot(n,p_0)/n[2]]) # TODO: Confirm divs. return w def _points_in_window(self, point_cloud_image, action, segmask=None): - """Retrieve all points on the object in a disc of size self._window_size. """ - # compute plane + """Retrieve all points on the object in a disc of size + `self._window_size`.""" + # Compute plane. n = -action.axis U, _, _ = np.linalg.svd(n.reshape((3,1))) tangents = U[:,1:] - # read indices + # Read indices. im_shape = point_cloud_image.shape - i_start = int(max(action.center.y-self._window_size/2, 0)) - j_start = int(max(action.center.x-self._window_size/2, 0)) + i_start = int(max(action.center.y-self._window_size//2, 0)) + j_start = int(max(action.center.x-self._window_size//2, 0)) i_end = int(min(i_start+self._window_size, im_shape[0])) j_end = int(min(j_start+self._window_size, im_shape[1])) step = int(1 / self._sample_rate) - # read 3D points in the window + # Read 3D points in the window. points = point_cloud_image[i_start:i_end:step, j_start:j_end:step] stacked_points = points.reshape(points.shape[0]*points.shape[1], -1) - # compute the center point + # Compute the center point. contact_point = point_cloud_image[int(action.center.y), int(action.center.x)] - # project onto approach plane + # Project onto approach plane. residuals = stacked_points - contact_point coords = residuals.dot(tangents) proj_residuals = coords.dot(tangents.T) - # check distance from the center point along the approach plane + # Check distance from the center point along the approach plane. dists = np.linalg.norm(proj_residuals, axis=1) stacked_points = stacked_points[dists <= self._radius] - # form the matrices for plane-fitting + # Form the matrices for plane-fitting. return stacked_points def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, + approach direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ qualities = [] - # deproject points + # Deproject points. point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) - # compute negative SSE from the best fit plane for each grasp + # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError('This function can only be used to evaluate suction quality') + raise ValueError("This function can only be used to evaluate suction quality") points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. A, b = self._points_to_matrices(points) w = self._action_to_plane(point_cloud_image, action) # vector w w/ a bias term represents a best-fit plane. sse = self._sum_of_squared_residuals(w, A, b) - if params is not None and params['vis']['plane']: + if params is not None and params["vis"]["plane"]: from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d - mid_i = A.shape[0] / 2 + mid_i = A.shape[0] // 2 pred_z = A.dot(w) p0 = np.array([A[mid_i,0], A[mid_i,1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) @@ -486,8 +503,8 @@ def quality(self, state, actions, params=None): T_table_world = RigidTransform(rotation=R, translation=p0, - from_frame='patch', - to_frame='world') + from_frame="patch", + to_frame="world") vis3d.figure() vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0,0,1)) @@ -499,55 +516,58 @@ def quality(self, state, actions, params=None): vis2d.figure() vis2d.imshow(state.rgbd_im.depth) - vis2d.scatter(action.center.x, action.center.y, s=50, c='b') + vis2d.scatter(action.center.x, action.center.y, s=50, c="b") vis2d.show() - quality = np.exp(-sse) # evaluate how well best-fit plane describles all points in window. + quality = np.exp(-sse) # Evaluate how well best-fit plane describles all points in window. qualities.append(quality) return np.array(qualities) class ComApproachPlanaritySuctionQualityFunction(ApproachPlanaritySuctionQualityFunction): - """A approach planarity suction metric that ranks sufficiently planar points by their distance to the object COM. """ + """A approach planarity suction metric that ranks sufficiently planar + points by their distance to the object COM.""" def __init__(self, config): - """Create approach planarity suction metric. """ - self._planarity_thresh = config['planarity_thresh'] + """Create approach planarity suction metric.""" + self._planarity_thresh = config["planarity_thresh"] ApproachPlanaritySuctionQualityFunction.__init__(self, config) def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ - # compute planarity + # Compute planarity. sse = ApproachPlanaritySuctionQualityFunction.quality(self, state, actions, params=params) - if params['vis']['hist']: + if params["vis"]["hist"]: plt.figure() utils.histogram(sse, 100, (np.min(sse), np.max(sse)), normalized=False, plot=True) plt.show() - # compute object centroid + # Compute object centroid. object_com = state.rgbd_im.center if state.segmask is not None: nonzero_px = state.segmask.nonzero_pixels() object_com = np.mean(nonzero_px, axis=0) - # threshold + # Threshold. qualities = [] for k, action in enumerate(actions): q = max(state.rgbd_im.height, state.rgbd_im.width) @@ -560,49 +580,52 @@ def quality(self, state, actions, params=None): return np.array(qualities) class ComDiscApproachPlanaritySuctionQualityFunction(DiscApproachPlanaritySuctionQualityFunction): - """A approach planarity suction metric that ranks sufficiently planar points by their distance to the object COM. """ + """A approach planarity suction metric that ranks sufficiently planar + points by their distance to the object COM.""" def __init__(self, config): - """Create approach planarity suction metric. """ - self._planarity_pctile = config['planarity_pctile'] + """Create approach planarity suction metric.""" + self._planarity_pctile = config["planarity_pctile"] self._planarity_abs_thresh = 0 - if 'planarity_abs_thresh' in config.keys(): - self._planarity_abs_thresh = np.exp(-config['planarity_abs_thresh']) + if "planarity_abs_thresh" in config: + self._planarity_abs_thresh = np.exp(-config["planarity_abs_thresh"]) DiscApproachPlanaritySuctionQualityFunction.__init__(self, config) def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ - # compute planarity + # Compute planarity. sse_q = DiscApproachPlanaritySuctionQualityFunction.quality(self, state, actions, params=params) - if params['vis']['hist']: + if params["vis"]["hist"]: plt.figure() utils.histogram(sse_q, 100, (np.min(sse_q), np.max(sse_q)), normalized=False, plot=True) plt.show() - # compute object centroid + # Compute object centroid. object_com = state.rgbd_im.center if state.segmask is not None: nonzero_px = state.segmask.nonzero_pixels() object_com = np.mean(nonzero_px, axis=0) - # threshold + # Threshold. planarity_thresh = abs(np.percentile(sse_q, 100-self._planarity_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) @@ -618,49 +641,52 @@ def quality(self, state, actions, params=None): return np.array(qualities) class ComDiscApproachPlanaritySuctionQualityFunction(DiscApproachPlanaritySuctionQualityFunction): - """A approach planarity suction metric that ranks sufficiently planar points by their distance to the object COM. """ + """A approach planarity suction metric that ranks sufficiently planar + points by their distance to the object COM.""" def __init__(self, config): - """Create approach planarity suction metric. """ - self._planarity_pctile = config['planarity_pctile'] + """Create approach planarity suction metric.""" + self._planarity_pctile = config["planarity_pctile"] self._planarity_abs_thresh = 0 - if 'planarity_abs_thresh' in config.keys(): - self._planarity_abs_thresh = np.exp(-config['planarity_abs_thresh']) + if "planarity_abs_thresh" in config: + self._planarity_abs_thresh = np.exp(-config["planarity_abs_thresh"]) DiscApproachPlanaritySuctionQualityFunction.__init__(self, config) def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ - # compute planarity + # Compute planarity. sse_q = DiscApproachPlanaritySuctionQualityFunction.quality(self, state, actions, params=params) - if params['vis']['hist']: + if params["vis"]["hist"]: plt.figure() utils.histogram(sse_q, 100, (np.min(sse_q), np.max(sse_q)), normalized=False, plot=True) plt.show() - # compute object centroid + # Compute object centroid. object_com = state.rgbd_im.center if state.segmask is not None: nonzero_px = state.segmask.nonzero_pixels() object_com = np.mean(nonzero_px, axis=0) - # threshold + # Threshold. planarity_thresh = abs(np.percentile(sse_q, 100-self._planarity_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) @@ -684,14 +710,14 @@ def quality(self, state, actions, params=None): return np.array(qualities) class GaussianCurvatureSuctionQualityFunction(SuctionQualityFunction): - """A approach planarity suction metric. """ + """A approach planarity suction metric.""" def __init__(self, config): - """Create approach planarity suction metric. """ + """Create approach planarity suction metric.""" SuctionQualityFunction.__init__(self, config) def _points_to_matrices(self, points): - """ Convert a set of 3D points to an A and b matrix for regression. """ + """Convert a set of 3D points to an A and b matrix for regression.""" x = points[:,0] y = points[:,1] A = np.c_[x, y, x*x, x*y, y*y] @@ -701,37 +727,39 @@ def _points_to_matrices(self, points): return A, b def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. + """Given a suction point, compute a score based on a best-fit 3D plane + of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ qualities = [] - # deproject points + # Deproject points. point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) - # compute negative SSE from the best fit plane for each grasp + # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError('This function can only be used to evaluate suction quality') + raise ValueError("This function can only be used to evaluate suction quality") points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. A, b = self._points_to_matrices(points) w = self._best_fit_plane(A, b) # vector w w/ a bias term represents a best-fit plane. - # compute curvature + # Compute curvature. fx = w[0] fy = w[1] fxx = 2 * w[2] @@ -739,7 +767,7 @@ def quality(self, state, actions, params=None): fyy = 2 * w[4] curvature = (fxx * fyy - fxy**2) / ((1 + fx**2 + fy**2)**2) - # store quality + # Store quality. quality = np.exp(-np.abs(curvature)) qualities.append(quality) @@ -747,72 +775,76 @@ def quality(self, state, actions, params=None): class DiscCurvatureSuctionQualityFunction(GaussianCurvatureSuctionQualityFunction): def __init__(self, config): - """Create approach planarity suction metric. """ - self._radius = config['radius'] + """Create approach planarity suction metric.""" + self._radius = config["radius"] SuctionQualityFunction.__init__(self, config) def _points_in_window(self, point_cloud_image, action, segmask=None): - """Retrieve all points on the object in a disc of size self._window_size. """ - # read indices + """Retrieve all points on the object in a disc of size + `self._window_size`. + """ + # Read indices. im_shape = point_cloud_image.shape - i_start = int(max(action.center.y-self._window_size/2, 0)) - j_start = int(max(action.center.x-self._window_size/2, 0)) + i_start = int(max(action.center.y-self._window_size//2, 0)) + j_start = int(max(action.center.x-self._window_size//2, 0)) i_end = int(min(i_start+self._window_size, im_shape[0])) j_end = int(min(j_start+self._window_size, im_shape[1])) step = int(1 / self._sample_rate) - # read 3D points in the window + # Read 3D points in the window. points = point_cloud_image[i_start:i_end:step, j_start:j_end:step] stacked_points = points.reshape(points.shape[0]*points.shape[1], -1) - # check the distance from the center point + # Check the distance from the center point. contact_point = point_cloud_image[int(action.center.y), int(action.center.x)] dists = np.linalg.norm(stacked_points - contact_point, axis=1) stacked_points = stacked_points[dists <= self._radius] - # form the matrices for plane-fitting + # Form the matrices for plane-fitting. return stacked_points class ComDiscCurvatureSuctionQualityFunction(DiscCurvatureSuctionQualityFunction): def __init__(self, config): - """Create approach planarity suction metric. """ - self._curvature_pctile = config['curvature_pctile'] + """Create approach planarity suction metric.""" + self._curvature_pctile = config["curvature_pctile"] DiscCurvatureSuctionQualityFunction.__init__(self, config) def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on the Gaussian curvature. + """Given a suction point, compute a score based on the Gaussian + curvature. Parameters ---------- state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. + An RgbdImageState instance that encapsulates rgbd_im, camera_intr, + segmask, full_observed. action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. + A suction grasp in image space that encapsulates center, approach + direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray` - Array of the quality for each grasp + :obj:`numpy.ndarray`: Array of the quality for each grasp. """ - # compute planarity + # Compute planarity. curvature_q = DiscCurvatureSuctionQualityFunction.quality(self, state, actions, params=params) - if params['vis']['hist']: + if params["vis"]["hist"]: plt.figure() utils.histogram(curvature, 100, (np.min(curvature), np.max(curvature)), normalized=False, plot=True) plt.show() - # compute object centroid + # Compute object centroid. object_com = state.rgbd_im.center if state.segmask is not None: nonzero_px = state.segmask.nonzero_pixels() object_com = np.mean(nonzero_px, axis=0) - # threshold + # Threshold. curvature_q_thresh = abs(np.percentile(curvature_q, 100-self._curvature_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) @@ -829,19 +861,19 @@ def quality(self, state, actions, params=None): class GQCnnQualityFunction(GraspQualityFunction): def __init__(self, config): - """Create a GQCNN suction quality function. """ + """Create a GQCNN suction quality function.""" GraspQualityFunction.__init__(self) - # store parameters + # Store parameters. self._config = config - self._gqcnn_model_dir = config['gqcnn_model'] - self._crop_height = config['crop_height'] - self._crop_width = config['crop_width'] + self._gqcnn_model_dir = config["gqcnn_model"] + self._crop_height = config["crop_height"] + self._crop_width = config["crop_width"] - # init GQ-CNN + # Init GQ-CNN self._gqcnn = get_gqcnn_model().load(self._gqcnn_model_dir) - # open tensorflow session for gqcnn + # Open Tensorflow session for gqcnn. self._gqcnn.open_session() def __del__(self): @@ -853,7 +885,7 @@ def __del__(self): @property def gqcnn(self): - """ Returns the GQ-CNN. """ + """Returns the GQ-CNN.""" return self._gqcnn @property @@ -870,7 +902,7 @@ def gqcnn_stride(self): @property def config(self): - """ Returns the GQCNN quality function parameters. """ + """Returns the GQCNN quality function parameters.""" return self._config def grasps_to_tensors(self, grasps, state): @@ -880,18 +912,18 @@ def grasps_to_tensors(self, grasps, state): Attributes ---------- grasps : :obj:`list` of :obj:`object` - list of image grasps to convert + List of image grasps to convert. state : :obj:`RgbdImageState` - RGB-D image to plan grasps on + RGB-D image to plan grasps on. Returns ------- image_arr : :obj:`numpy.ndarray` - 4D numpy tensor of image to be predicted + 4D numpy tensor of image to be predicted. pose_arr : :obj:`numpy.ndarray` - 2D numpy tensor of depth values + 2D numpy tensor of depth values. """ - # parse params + # Parse params. gqcnn_im_height = self.gqcnn.im_height gqcnn_im_width = self.gqcnn.im_width gqcnn_num_channels = self.gqcnn.num_channels @@ -900,11 +932,11 @@ def grasps_to_tensors(self, grasps, state): num_grasps = len(grasps) depth_im = state.rgbd_im.depth - # allocate tensors + # Allocate tensors. tensor_start = time() image_tensor = np.zeros([num_grasps, gqcnn_im_height, gqcnn_im_width, gqcnn_num_channels]) pose_tensor = np.zeros([num_grasps, gqcnn_pose_dim]) - scale = float(gqcnn_im_height) / self._crop_height + scale = gqcnn_im_height / self._crop_height depth_im_scaled = depth_im.resize(scale) for i, grasp in enumerate(grasps): translation = scale * np.array([depth_im.center[0] - grasp.center.data[1], @@ -925,94 +957,95 @@ def grasps_to_tensors(self, grasps, state): elif gripper_mode == GripperMode.LEGACY_SUCTION: pose_tensor[i,...] = np.array([grasp.depth, grasp.approach_angle]) else: - raise ValueError('Gripper mode %s not supported' %(gripper_mode)) - self._logger.debug('Tensor conversion took %.3f sec' %(time()-tensor_start)) + raise ValueError("Gripper mode %s not supported" %(gripper_mode)) + self._logger.debug("Tensor conversion took %.3f sec" %(time()-tensor_start)) return image_tensor, pose_tensor def quality(self, state, actions, params): - """ Evaluate the quality of a set of actions according to a GQ-CNN. + """Evaluate the quality of a set of actions according to a GQ-CNN. Parameters ---------- state : :obj:`RgbdImageState` - state of the world described by an RGB-D image + State of the world described by an RGB-D image. actions: :obj:`object` - set of grasping actions to evaluate + Set of grasping actions to evaluate. params: dict - optional parameters for quality evaluation + Optional parameters for quality evaluation. Returns ------- - :obj:`list` of float - real-valued grasp quality predictions for each action, between 0 and 1 + :obj:`list` of float: Real-valued grasp quality predictions for each + action, between 0 and 1. """ - # form tensors + # Form tensors. tensor_start = time() image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) - self._logger.info('Image transformation took %.3f sec' %(time() - tensor_start)) - if params is not None and params['vis']['tf_images']: - # read vis params - k = params['vis']['k'] + self._logger.info("Image transformation took %.3f sec" %(time() - tensor_start)) + if params is not None and params["vis"]["tf_images"]: + # Read vis params. + k = params["vis"]["k"] d = utils.sqrt_ceil(k) - # display grasp transformed images + # Display grasp transformed images. from visualization import Visualizer2D as vis2d - vis2d.figure(size=(FIGSIZE,FIGSIZE)) + vis2d.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) for i, image_tf in enumerate(image_tensor[:k,...]): depth = pose_tensor[i][0] vis2d.subplot(d,d,i+1) vis2d.imshow(DepthImage(image_tf)) - vis2d.title('Image %d: d=%.3f' %(i, depth)) + vis2d.title("Image %d: d=%.3f" %(i, depth)) vis2d.show() - # predict grasps + # Predict grasps. predict_start = time() output_arr = self.gqcnn.predict(image_tensor, pose_tensor) q_values = output_arr[:,-1] - self._logger.info('Inference took %.3f sec' %(time() - predict_start)) + self._logger.info("Inference took %.3f sec" %(time() - predict_start)) return q_values.tolist() class NoMagicQualityFunction(GraspQualityFunction): def __init__(self, config): - """Create a quality that uses nomagic_net as a quality function. """ + """Create a quality that uses `nomagic_net` as a quality function.""" + import json + import os + from nomagic_submission import ConvNetModel - from tensorpack.predict.config import PredictConfig from tensorpack import SaverRestore from tensorpack.predict import OfflinePredictor - import json - import os - + from tensorpack.predict.config import PredictConfig + GraspQualityFunction.__init(self) - # store parameters - self._model_path = config['gqcnn_model'] - self._batch_size = config['batch_size'] - self._crop_height = config['crop_height'] - self._crop_width = config['crop_width'] - self._im_height = config['im_height'] - self._im_width = config['im_width'] - self._num_channels = config['num_channels'] - self._pose_dim = config['pose_dim'] - self._gripper_mode = config['gripper_mode'] - self._data_mean = config['data_mean'] - self._data_std = config['data_std'] - - # init config + # Store parameters. + self._model_path = config["gqcnn_model"] + self._batch_size = config["batch_size"] + self._crop_height = config["crop_height"] + self._crop_width = config["crop_width"] + self._im_height = config["im_height"] + self._im_width = config["im_width"] + self._num_channels = config["num_channels"] + self._pose_dim = config["pose_dim"] + self._gripper_mode = config["gripper_mode"] + self._data_mean = config["data_mean"] + self._data_std = config["data_std"] + + # Init config. model = ConvNetModel() self._config = PredictConfig( model=model, session_init=SaverRestore(self._model_path), - output_names=['prob']) + output_names=["prob"]) self._predictor = OfflinePredictor(self._config) @property def gqcnn(self): - """ Returns the GQ-CNN. """ + """Returns the GQ-CNN.""" return self._predictor @property def config(self): - """ Returns the GQCNN suction quality function parameters. """ + """Returns the GQCNN suction quality function parameters.""" return self._config def grasps_to_tensors(self, grasps, state): @@ -1022,18 +1055,16 @@ def grasps_to_tensors(self, grasps, state): Attributes ---------- grasps : :obj:`list` of :obj:`object` - list of image grasps to convert + List of image grasps to convert. state : :obj:`RgbdImageState` - RGB-D image to plan grasps on + RGB-D image to plan grasps on. Returns ------- - image_arr : :obj:`numpy.ndarray` - 4D numpy tensor of image to be predicted - pose_arr : :obj:`numpy.ndarray` - 2D numpy tensor of depth values + :obj:`numpy.ndarray`: 4D numpy tensor of image to be predicted. + :obj:`numpy.ndarray`: 2D numpy tensor of depth values. """ - # parse params + # Parse params. gqcnn_im_height = self._im_height gqcnn_im_width = self._im_width gqcnn_num_channels = self._num_channels @@ -1042,12 +1073,12 @@ def grasps_to_tensors(self, grasps, state): num_grasps = len(grasps) depth_im = state.rgbd_im.depth - # allocate tensors + # Allocate tensors. tensor_start = time() image_tensor = np.zeros([num_grasps, gqcnn_im_height, gqcnn_im_width, gqcnn_num_channels]) pose_tensor = np.zeros([num_grasps, gqcnn_pose_dim]) - scale = float(gqcnn_im_height) / self._crop_height + scale = gqcnn_im_height / self._crop_height depth_im_scaled = depth_im.resize(scale) for i, grasp in enumerate(grasps): translation = scale * np.array([depth_im.center[0] - grasp.center.data[1], @@ -1056,7 +1087,7 @@ def grasps_to_tensors(self, grasps, state): im_tf = depth_im_scaled.transform(translation, grasp.angle) im_tf = im_tf.crop(gqcnn_im_height, gqcnn_im_width) - im_encoded = cv2.imencode('.png', np.uint8(im_tf.raw_data*255))[1].tostring() + im_encoded = cv2.imencode(".png", np.uint8(im_tf.raw_data*255))[1].tostring() im_decoded = cv2.imdecode(np.frombuffer(im_encoded, np.uint8), 0) / 255.0 image_tensor[i,:,:,0] = ((im_decoded - self._data_mean) / self._data_std) @@ -1069,45 +1100,44 @@ def grasps_to_tensors(self, grasps, state): elif gripper_mode == GripperMode.LEGACY_SUCTION: pose_tensor[i,...] = np.array([grasp.depth, grasp.approach_angle]) else: - raise ValueError('Gripper mode %s not supported' %(gripper_mode)) - self._logger.debug('Tensor conversion took %.3f sec' %(time()-tensor_start)) + raise ValueError("Gripper mode %s not supported" %(gripper_mode)) + self._logger.debug("Tensor conversion took %.3f sec" %(time()-tensor_start)) return image_tensor, pose_tensor def quality(self, state, actions, params): - """ Evaluate the quality of a set of actions according to a GQ-CNN. + """Evaluate the quality of a set of actions according to a GQ-CNN. Parameters ---------- state : :obj:`RgbdImageState` - state of the world described by an RGB-D image + State of the world described by an RGB-D image. actions: :obj:`object` - set of grasping actions to evaluate + Set of grasping actions to evaluate. params: dict - optional parameters for quality evaluation + Optional parameters for quality evaluation. Returns ------- - :obj:`list` of float - real-valued grasp quality predictions for each action, between 0 and 1 + :obj:`list` of float: Real-valued grasp quality predictions for each action, between 0 and 1. """ - # form tensors + # Form tensors. image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) - if params is not None and params['vis']['tf_images']: - # read vis params - k = params['vis']['k'] + if params is not None and params["vis"]["tf_images"]: + # Read vis params. + k = params["vis"]["k"] d = utils.sqrt_ceil(k) - # display grasp transformed images + # Display grasp transformed images. from visualization import Visualizer2D as vis2d - vis2d.figure(size=(FIGSIZE,FIGSIZE)) + vis2d.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) for i, image_tf in enumerate(image_tensor[:k,...]): depth = pose_tensor[i][0] vis2d.subplot(d,d,i+1) vis2d.imshow(DepthImage(image_tf)) - vis2d.title('Image %d: d=%.3f' %(i, depth)) + vis2d.title("Image %d: d=%.3f" %(i, depth)) vis2d.show() - # predict grasps + # Predict grasps. num_actions = len(actions) null_arr = -1 * np.ones(self._batch_size) predict_start = time() @@ -1119,24 +1149,24 @@ def quality(self, state, actions, params): cur_i = end_i end_i = cur_i + min(self._batch_size, num_actions - cur_i) q_values = output_arr[:,-1] - self._logger.debug('Prediction took %.3f sec' %(time()-predict_start)) + self._logger.debug("Prediction took %.3f sec" %(time()-predict_start)) return q_values.tolist() class FCGQCnnQualityFunction(GraspQualityFunction): def __init__(self, config): - """ Grasp quality function using the fully-convolutional gqcnn """ + """Grasp quality function using the fully-convolutional gqcnn.""" GraspQualityFunction.__init__(self) - # store parameters + # Store parameters. self._config = config - self._model_dir = config['gqcnn_model'] - self._backend = config['gqcnn_backend'] - self._fully_conv_config = config['fully_conv_gqcnn_config'] + self._model_dir = config["gqcnn_model"] + self._backend = config["gqcnn_backend"] + self._fully_conv_config = config["fully_conv_gqcnn_config"] - # init fcgqcnn + # Init fcgqcnn. self._fcgqcnn = get_fc_gqcnn_model(backend=self._backend).load(self._model_dir, self._fully_conv_config) - # open tensorflow session for fcgqcnn + # Open Tensorflow session for fcgqcnn. self._fcgqcnn.open_session() def __del__(self): @@ -1148,46 +1178,46 @@ def __del__(self): @property def gqcnn(self): - """ Returns the FC-GQCNN. """ + """Returns the FC-GQCNN.""" return self._fcgqcnn @property def config(self): - """ Returns the FC-GQCNN quality function parameters. """ + """Returns the FC-GQCNN quality function parameters.""" return self._config def quality(self, images, depths, params=None): return self._fcgqcnn.predict(images, depths) class GraspQualityFunctionFactory(object): - """Factory for grasp quality functions. """ + """Factory for grasp quality functions.""" @staticmethod def quality_function(metric_type, config): - if metric_type == 'zero': + if metric_type == "zero": return ZeroGraspQualityFunction() - elif metric_type == 'parallel_jaw_com_force_closure': + elif metric_type == "parallel_jaw_com_force_closure": return ComForceClosureParallelJawQualityFunction(config) - elif metric_type == 'suction_best_fit_planarity': + elif metric_type == "suction_best_fit_planarity": return BestFitPlanaritySuctionQualityFunction(config) - elif metric_type == 'suction_approach_planarity': + elif metric_type == "suction_approach_planarity": return ApproachPlanaritySuctionQualityFunction(config) - elif metric_type == 'suction_com_approach_planarity': + elif metric_type == "suction_com_approach_planarity": return ComApproachPlanaritySuctionQualityFunction(config) - elif metric_type == 'suction_disc_approach_planarity': + elif metric_type == "suction_disc_approach_planarity": return DiscApproachPlanaritySuctionQualityFunction(config) - elif metric_type == 'suction_com_disc_approach_planarity': + elif metric_type == "suction_com_disc_approach_planarity": return ComDiscApproachPlanaritySuctionQualityFunction(config) - elif metric_type == 'suction_gaussian_curvature': + elif metric_type == "suction_gaussian_curvature": return GaussianCurvatureSuctionQualityFunction(config) - elif metric_type == 'suction_disc_curvature': + elif metric_type == "suction_disc_curvature": return DiscCurvatureSuctionQualityFunction(config) - elif metric_type == 'suction_com_disc_curvature': + elif metric_type == "suction_com_disc_curvature": return ComDiscCurvatureSuctionQualityFunction(config) - elif metric_type == 'gqcnn': + elif metric_type == "gqcnn": return GQCnnQualityFunction(config) - elif metric_type == 'nomagic': + elif metric_type == "nomagic": return NoMagicQualityFunction(config) - elif metric_type == 'fcgqcnn': + elif metric_type == "fcgqcnn": return FCGQCnnQualityFunction(config) else: - raise ValueError('Grasp function type %s not supported!' %(metric_type)) + raise ValueError("Grasp function type %s not supported!" %(metric_type)) diff --git a/gqcnn/grasping/image_grasp_sampler.py b/gqcnn/grasping/image_grasp_sampler.py index 4c34fe69..99162beb 100644 --- a/gqcnn/grasping/image_grasp_sampler.py +++ b/gqcnn/grasping/image_grasp_sampler.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,38 +21,45 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Classes for sampling a set of grasps directly from images to generate data for a neural network + +Classes for sampling a set of grasps directly from images to generate data for +a neural network. Author: Jeff Mahler, Sherdil Niyaz """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from abc import ABCMeta, abstractmethod import copy import os import random from time import sleep, time -import scipy.spatial.distance as ssd +from future.utils import with_metaclass +import matplotlib.pyplot as plt +import numpy as np import scipy.ndimage.filters as snf +import scipy.spatial.distance as ssd import scipy.stats as ss import sklearn.mixture -import numpy as np -import matplotlib.pyplot as plt from autolab_core import Point, RigidTransform, Logger -from perception import BinaryImage, ColorImage, DepthImage, RgbdImage, GdImage +from perception import (BinaryImage, ColorImage, DepthImage, + RgbdImage, GdImage) from visualization import Visualizer2D as vis -from gqcnn.grasping import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D -from gqcnn.utils import NoAntipodalPairsFoundException +from .grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D +from ..utils import NoAntipodalPairsFoundException def force_closure(p1, p2, n1, n2, mu): - """ Computes whether or not the point and normal pairs are in force closure. """ - # line between the contacts + """Computes whether or not the point and normal pairs are in force + closure.""" + # Line between the contacts. v = p2 - p1 v = v / np.linalg.norm(v) - # compute cone membership + # Compute cone membership. alpha = np.arctan(mu) dot_1 = max(min(n1.dot(-v), 1.0), -1.0) dot_2 = max(min(n2.dot(v), 1.0), -1.0) @@ -58,187 +68,187 @@ def force_closure(p1, p2, n1, n2, mu): return (in_cone_1 and in_cone_2) class DepthSamplingMode(object): - """ Modes for sampling grasp depth. """ - UNIFORM = 'uniform' - MIN = 'min' - MAX = 'max' + """Modes for sampling grasp depth.""" + UNIFORM = "uniform" + MIN = "min" + MAX = "max" -class ImageGraspSampler(object): - """ - Wraps image to crane grasp candidate generation for easy deployment of GQ-CNN. +class ImageGraspSampler(with_metaclass(ABCMeta, object)): + """Wraps image to crane grasp candidate generation for easy deployment of + GQ-CNN. Attributes ---------- config : :obj:`autolab_core.YamlConfig` - a dictionary-like object containing the parameters of the sampler + A dictionary-like object containing the parameters of the sampler. """ - __metaclass__ = ABCMeta - def __init__(self, config): - # set params + # Set params. self._config = config - # setup logger + # Setup logger. self._logger = Logger.get_logger(self.__class__.__name__) def sample(self, rgbd_im, camera_intr, num_samples, segmask=None, seed=None, visualize=False, constraint_fn=None): - """ - Samples a set of 2D grasps from a given RGB-D image. + """Samples a set of 2D grasps from a given RGB-D image. Parameters ---------- rgbd_im : :obj:`perception.RgbdImage` - RGB-D image to sample from + RGB-D image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. seed : int - number to use in random seed (None if no seed) + Number to use in random seed (`None` if no seed). visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` - constraint function to apply to grasps + Constraint function to apply to grasps. Returns ------- - :obj:`list` of :obj:`Grasp2D` - the list of grasps in image space + :obj:`list` of :obj:`Grasp2D`: The list of grasps in image space. """ - # set random seed for determinism + # Set random seed for determinism. if seed is not None: random.seed(seed) np.random.seed(seed) - # sample an initial set of grasps (without depth) - self._logger.debug('Sampling 2d candidates') + # Sample an initial set of grasps (without depth). + self._logger.debug("Sampling 2d candidates") sampling_start = time() grasps = self._sample(rgbd_im, camera_intr, num_samples, segmask=segmask, visualize=visualize, constraint_fn=constraint_fn) sampling_stop = time() - self._logger.debug('Sampled %d grasps from image' %(len(grasps))) - self._logger.debug('Sampling grasps took %.3f sec' %(sampling_stop - sampling_start)) + self._logger.debug("Sampled %d grasps from image" %(len(grasps))) + self._logger.debug("Sampling grasps took %.3f sec" %(sampling_stop - sampling_start)) return grasps @abstractmethod def _sample(self, rgbd_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D grasp candidates from a depth image. + """Sample a set of 2D grasp candidates from a depth image. + Subclasses must override. Parameters ---------- rgbd_im : :obj:`perception.RgbdImage` - RGB-D image to sample from + RGB-D image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` - constraint function to apply to grasps + Constraint function to apply to grasps. Returns ------- - :obj:`list` of :obj:`Grasp2D` - list of 2D grasp candidates + :obj:`list` of :obj:`Grasp2D`: List of 2D grasp candidates. """ pass class AntipodalDepthImageGraspSampler(ImageGraspSampler): - """ Grasp sampler for antipodal point pairs from depth image gradients. + """Grasp sampler for antipodal point pairs from depth image gradients. Notes ----- - Required configuration parameters are specified in Other Parameters + Required configuration parameters are specified in Other Parameters. Other Parameters ---------------- gripper_width : float - width of the gripper, in meters + Width of the gripper, in meters. friction_coef : float - friction coefficient for 2D force closure + Friction coefficient for 2D force closure. depth_grad_thresh : float - threshold for depth image gradients to determine edge points for sampling + Threshold for depth image gradients to determine edge points for + sampling. depth_grad_gaussian_sigma : float - sigma used for pre-smoothing the depth image for better gradients + Sigma used for pre-smoothing the depth image for better gradients. downsample_rate : float - factor to downsample the depth image by before sampling grasps + Factor to downsample the depth image by before sampling grasps. max_rejection_samples : int - ceiling on the number of grasps to check in antipodal grasp rejection sampling + Ceiling on the number of grasps to check in antipodal grasp rejection + sampling. max_dist_from_center : int - maximum allowable distance of a grasp from the image center + Maximum allowable distance of a grasp from the image center. min_grasp_dist : float - threshold on the grasp distance + Threshold on the grasp distance. angle_dist_weight : float - amount to weight the angle difference in grasp distance computation + Amount to weight the angle difference in grasp distance computation. depth_samples_per_grasp : int - number of depth samples to take per grasp + Number of depth samples to take per grasp. min_depth_offset : float - offset from the minimum depth at the grasp center pixel to use in depth sampling + Offset from the minimum depth at the grasp center pixel to use in depth + sampling. max_depth_offset : float - offset from the maximum depth across all edges + Offset from the maximum depth across all edges. depth_sample_win_height : float - height of a window around the grasp center pixel used to determine min depth + Height of a window around the grasp center pixel used to determine min + depth. depth_sample_win_height : float - width of a window around the grasp center pixel used to determine min depth + Width of a window around the grasp center pixel used to determine min + depth. depth_sampling_mode : str - name of depth sampling mode (uniform, min, max) + Name of depth sampling mode (uniform, min, max). """ def __init__(self, config, gripper_width=np.inf): - # init superclass + # Init superclass. ImageGraspSampler.__init__(self, config) - # antipodality params - self._gripper_width = self._config['gripper_width'] - self._friction_coef = self._config['friction_coef'] - self._depth_grad_thresh = self._config['depth_grad_thresh'] - self._depth_grad_gaussian_sigma = self._config['depth_grad_gaussian_sigma'] - self._downsample_rate = self._config['downsample_rate'] + # Antipodality params. + self._gripper_width = self._config["gripper_width"] + self._friction_coef = self._config["friction_coef"] + self._depth_grad_thresh = self._config["depth_grad_thresh"] + self._depth_grad_gaussian_sigma = self._config["depth_grad_gaussian_sigma"] + self._downsample_rate = self._config["downsample_rate"] self._rescale_factor = 1.0 / self._downsample_rate - self._max_rejection_samples = self._config['max_rejection_samples'] + self._max_rejection_samples = self._config["max_rejection_samples"] self._min_num_edge_pixels = 0 - if 'min_num_edge_pixels' in self._config.keys(): - self._min_num_edge_pixels = self._config['min_num_edge_pixels'] + if "min_num_edge_pixels" in self._config: + self._min_num_edge_pixels = self._config["min_num_edge_pixels"] - # distance thresholds for rejection sampling - self._max_dist_from_center = self._config['max_dist_from_center'] - self._min_dist_from_boundary = self._config['min_dist_from_boundary'] - self._min_grasp_dist = self._config['min_grasp_dist'] - self._angle_dist_weight = self._config['angle_dist_weight'] - - # depth sampling params - self._depth_samples_per_grasp = max(self._config['depth_samples_per_grasp'], 1) - self._min_depth_offset = self._config['min_depth_offset'] - self._max_depth_offset = self._config['max_depth_offset'] - self._h = self._config['depth_sample_win_height'] - self._w = self._config['depth_sample_win_width'] - self._depth_sampling_mode = self._config['depth_sampling_mode'] - - # perturbation + # Distance thresholds for rejection sampling. + self._max_dist_from_center = self._config["max_dist_from_center"] + self._min_dist_from_boundary = self._config["min_dist_from_boundary"] + self._min_grasp_dist = self._config["min_grasp_dist"] + self._angle_dist_weight = self._config["angle_dist_weight"] + + # Depth sampling params. + self._depth_samples_per_grasp = max(self._config["depth_samples_per_grasp"], 1) + self._min_depth_offset = self._config["min_depth_offset"] + self._max_depth_offset = self._config["max_depth_offset"] + self._h = self._config["depth_sample_win_height"] + self._w = self._config["depth_sample_win_width"] + self._depth_sampling_mode = self._config["depth_sampling_mode"] + + # Perturbation. self._grasp_center_sigma = 0.0 - if 'grasp_center_sigma' in self._config.keys(): - self._grasp_center_sigma = self._config['grasp_center_sigma'] + if "grasp_center_sigma" in self._config: + self._grasp_center_sigma = self._config["grasp_center_sigma"] self._grasp_angle_sigma = 0.0 - if 'grasp_angle_sigma' in self._config.keys(): - self._grasp_angle_sigma = np.deg2rad(self._config['grasp_angle_sigma']) + if "grasp_angle_sigma" in self._config: + self._grasp_angle_sigma = np.deg2rad(self._config["grasp_angle_sigma"]) def _surface_normals(self, depth_im, edge_pixels): - """ Return an array of the surface normals at the edge pixels. """ - # compute the gradients + """Return an array of the surface normals at the edge pixels.""" + # Compute the gradients. grad = np.gradient(depth_im.data.astype(np.float32)) - # compute surface normals + # Compute surface normals. normals = np.zeros([edge_pixels.shape[0], 2]) for i, pix in enumerate(edge_pixels): dx = grad[1][pix[0], pix[1]] @@ -252,7 +262,7 @@ def _surface_normals(self, depth_im, edge_pixels): return normals def _sample_depth(self, min_depth, max_depth): - """ Samples a depth value between the min and max. """ + """Samples a depth value between the min and max.""" depth_sample = max_depth if self._depth_sampling_mode == DepthSamplingMode.UNIFORM: depth_sample = min_depth + (max_depth - min_depth) * np.random.rand() @@ -262,28 +272,27 @@ def _sample_depth(self, min_depth, max_depth): def _sample(self, image, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D grasp candidates from a depth image. + """Sample a set of 2D grasp candidates from a depth image. Parameters ---------- - image : :obj:`perception.RgbdImage` or 'perception.DepthImage' or 'perception.GdImage' + image : :obj:`perception.RgbdImage` or "perception.DepthImage" or "perception.GdImage" RGB-D or D image to sample from camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images num_samples : int - number of grasps to sample + Number of grasps to sample segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` - constraint function to apply to grasps + Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`Grasp2D` - list of 2D grasp candidates + List of 2D grasp candidates. """ if isinstance(image, RgbdImage) or isinstance(image, GdImage): depth_im = image.depth @@ -292,7 +301,7 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, else: raise ValueError("image type must be one of [RgbdImage, DepthImage, GdImage]") - # sample antipodal pairs in image space + # Sample antipodal pairs in image space. grasps = self._sample_antipodal_grasps(depth_im, camera_intr, num_samples, segmask=segmask, visualize=visualize, constraint_fn=constraint_fn) @@ -300,32 +309,31 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D grasp candidates from a depth image by finding depth - edges, then uniformly sampling point pairs and keeping only antipodal - grasps with width less than the maximum allowable. + """Sample a set of 2D grasp candidates from a depth image by finding + depth edges, then uniformly sampling point pairs and keeping only + antipodal grasps with width less than the maximum allowable. Parameters ---------- - depth_im : :obj:'perception.DepthImage' - Depth image to sample from + depth_im : :obj:"perception.DepthImage" + Depth image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` - constraint function to apply to grasps + Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`Grasp2D` - list of 2D grasp candidates + List of 2D grasp candidates. """ - # compute edge pixels + # Compute edge pixels. edge_start = time() depth_im = depth_im.apply(snf.gaussian_filter, sigma=self._depth_grad_gaussian_sigma) @@ -340,9 +348,9 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, edge_pixels = np.array([p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)]) depth_im_mask = depth_im.mask_binary(segmask) - # re-threshold edges if there are too few + # Re-threshold edges if there are too few. if edge_pixels.shape[0] < self._min_num_edge_pixels: - self._logger.info('Too few edge pixels!') + self._logger.info("Too few edge pixels!") depth_im_threshed = depth_im.threshold_gradients(self._depth_grad_thresh) edge_pixels = depth_im_threshed.zero_pixels() edge_pixels = edge_pixels.astype(np.int16) @@ -352,13 +360,13 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, depth_im_mask = depth_im.mask_binary(segmask) num_pixels = edge_pixels.shape[0] - self._logger.debug('Depth edge detection took %.3f sec' %(time() - edge_start)) - self._logger.debug('Found %d edge pixels' %(num_pixels)) + self._logger.debug("Depth edge detection took %.3f sec" %(time() - edge_start)) + self._logger.debug("Found %d edge pixels" %(num_pixels)) - # compute point cloud + # Compute point cloud. point_cloud_im = camera_intr.deproject_to_image(depth_im_mask) - # compute_max_depth + # Compute_max_depth. depth_data = depth_im_mask.data[depth_im_mask.data > 0] if depth_data.shape[0] == 0: return [] @@ -366,10 +374,10 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, min_depth = np.min(depth_data) + self._min_depth_offset max_depth = np.max(depth_data) + self._max_depth_offset - # compute surface normals + # Compute surface normals. normal_start = time() edge_normals = self._surface_normals(depth_im, edge_pixels) - self._logger.debug('Normal computation took %.3f sec' %(time() - normal_start)) + self._logger.debug("Normal computation took %.3f sec" %(time() - normal_start)) if visualize: edge_pixels = edge_pixels[::2,:] @@ -379,29 +387,29 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, vis.subplot(1,3,1) vis.imshow(depth_im) if num_pixels > 0: - vis.scatter(edge_pixels[:,1], edge_pixels[:,0], s=2, c='b') + vis.scatter(edge_pixels[:,1], edge_pixels[:,0], s=2, c="b") X = [pix[1] for pix in edge_pixels] Y = [pix[0] for pix in edge_pixels] U = [3*pix[1] for pix in edge_normals] V = [-3*pix[0] for pix in edge_normals] - plt.quiver(X, Y, U, V, units='x', scale=0.25, width=0.5, zorder=2, color='r') - vis.title('Edge pixels and normals') + plt.quiver(X, Y, U, V, units="x", scale=0.25, width=0.5, zorder=2, color="r") + vis.title("Edge pixels and normals") vis.subplot(1,3,2) vis.imshow(depth_im_threshed) - vis.title('Edge map') + vis.title("Edge map") vis.subplot(1,3,3) vis.imshow(segmask) - vis.title('Segmask') + vis.title("Segmask") vis.show() - # exit if no edge pixels + # Exit if no edge pixels. if num_pixels == 0: return [] - # form set of valid candidate point pairs + # Form set of valid candidate point pairs. pruning_start = time() max_grasp_width_px = Grasp2D(Point(np.zeros(2)), 0.0, min_depth, width = self._gripper_width, @@ -410,14 +418,14 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, dists = ssd.squareform(ssd.pdist(edge_pixels)) valid_indices = np.where((normal_ip < -np.cos(np.arctan(self._friction_coef))) & (dists < max_grasp_width_px) & (dists > 0.0)) valid_indices = np.c_[valid_indices[0], valid_indices[1]] - self._logger.debug('Normal pruning %.3f sec' %(time() - pruning_start)) + self._logger.debug("Normal pruning %.3f sec" %(time() - pruning_start)) - # raise exception if no antipodal pairs + # Raise exception if no antipodal pairs. num_pairs = valid_indices.shape[0] if num_pairs == 0: return [] - # prune out grasps + # Prune out grasps. contact_points1 = edge_pixels[valid_indices[:,0],:] contact_points2 = edge_pixels[valid_indices[:,1],:] contact_normals1 = edge_normals[valid_indices[:,0],:] @@ -436,7 +444,7 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, alpha = np.arctan(self._friction_coef) antipodal_indices = np.where((beta1 < alpha) & (beta2 < alpha))[0] - # raise exception if no antipodal pairs + # Raise exception if no antipodal pairs. num_pairs = antipodal_indices.shape[0] if num_pairs == 0: return [] @@ -444,9 +452,9 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, grasp_indices = np.random.choice(antipodal_indices, size=sample_size, replace=False) - self._logger.debug('Grasp comp took %.3f sec' %(time() - pruning_start)) + self._logger.debug("Grasp comp took %.3f sec" %(time() - pruning_start)) - # compute grasps + # Compute grasps. sample_start = time() k = 0 grasps = [] @@ -459,8 +467,8 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, width = np.linalg.norm(p1 - p2) k += 1 - # compute center and axis - grasp_center = (p1 + p2) / 2 + # Compute center and axis. + grasp_center = (p1 + p2) // 2 grasp_axis = p2 - p1 grasp_axis = grasp_axis / np.linalg.norm(grasp_axis) grasp_theta = np.pi / 2 @@ -468,34 +476,34 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, grasp_theta = np.arctan2(grasp_axis[0], grasp_axis[1]) grasp_center_pt = Point(np.array([grasp_center[1], grasp_center[0]]), frame=camera_intr.frame) - # compute grasp points in 3D + # Compute grasp points in 3D. x1 = point_cloud_im[p1[0], p1[1]] x2 = point_cloud_im[p2[0], p2[1]] if np.linalg.norm(x2-x1) > self._gripper_width: continue - # perturb + # Perturb. if self._grasp_center_sigma > 0.0: grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs(cov=self._grasp_center_sigma*np.diag(np.ones(2))) if self._grasp_angle_sigma > 0.0: grasp_theta = grasp_theta + ss.norm.rvs(scale=self._grasp_angle_sigma) - # check center px dist from boundary + # Check center px dist from boundary. if grasp_center[0] < self._min_dist_from_boundary or \ grasp_center[1] < self._min_dist_from_boundary or \ grasp_center[0] > depth_im.height - self._min_dist_from_boundary or \ grasp_center[1] > depth_im.width - self._min_dist_from_boundary: continue - # sample depths + # Sample depths. for i in range(self._depth_samples_per_grasp): - # get depth in the neighborhood of the center pixel + # Get depth in the neighborhood of the center pixel. depth_win = depth_im.data[grasp_center[0]-self._h:grasp_center[0]+self._h, grasp_center[1]-self._w:grasp_center[1]+self._w] center_depth = np.min(depth_win) if center_depth == 0 or np.isnan(center_depth): continue - # sample depth between the min and max + # Sample depth between the min and max. min_depth = center_depth + self._min_depth_offset max_depth = center_depth + self._max_depth_offset sample_depth = min_depth + (max_depth - min_depth) * np.random.rand() @@ -511,91 +519,96 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, vis.figure() vis.imshow(depth_im) vis.grasp(candidate_grasp) - vis.scatter(p1[1], p1[0], c='b', s=25) - vis.scatter(p2[1], p2[0], c='b', s=25) + vis.scatter(p1[1], p1[0], c="b", s=25) + vis.scatter(p2[1], p2[0], c="b", s=25) vis.show() grasps.append(candidate_grasp) - # return sampled grasps - self._logger.debug('Loop took %.3f sec' %(time() - sample_start)) + # Return sampled grasps. + self._logger.debug("Loop took %.3f sec" %(time() - sample_start)) return grasps class DepthImageSuctionPointSampler(ImageGraspSampler): - """ Grasp sampler for suction points from depth images. + """Grasp sampler for suction points from depth images. Notes ----- - Required configuration parameters are specified in Other Parameters + Required configuration parameters are specified in Other Parameters. Other Parameters ---------------- max_suction_dir_optical_axis_angle : float - maximum angle, in degrees, between the suction approach axis and the camera optical axis + Maximum angle, in degrees, between the suction approach axis and the + camera optical axis. delta_theta : float - maximum deviation from zero for the aziumth angle of a rotational perturbation to the surface normal (for sample diversity) + Maximum deviation from zero for the aziumth angle of a rotational + perturbation to the surface normal (for sample diversity). delta_phi : float - maximum deviation from zero for the elevation angle of a rotational perturbation to the surface normal (for sample diversity) + Maximum deviation from zero for the elevation angle of a rotational + perturbation to the surface normal (for sample diversity). sigma_depth : float - standard deviation for a normal distribution over depth values (for sample diversity) + Standard deviation for a normal distribution over depth values (for + sample diversity). min_suction_dist : float - minimum admissible distance between suction points (for sample diversity) + Minimum admissible distance between suction points (for sample + diversity). angle_dist_weight : float - amount to weight the angle difference in suction point distance computation + Amount to weight the angle difference in suction point distance + computation. depth_gaussian_sigma : float - sigma used for pre-smoothing the depth image for better gradients + Sigma used for pre-smoothing the depth image for better gradients. """ def __init__(self, config): - # init superclass + # Init superclass. ImageGraspSampler.__init__(self, config) - # read params - self._max_suction_dir_optical_axis_angle = np.deg2rad(self._config['max_suction_dir_optical_axis_angle']) - self._max_dist_from_center = self._config['max_dist_from_center'] - self._min_dist_from_boundary = self._config['min_dist_from_boundary'] - self._max_num_samples = self._config['max_num_samples'] + # Read params. + self._max_suction_dir_optical_axis_angle = np.deg2rad(self._config["max_suction_dir_optical_axis_angle"]) + self._max_dist_from_center = self._config["max_dist_from_center"] + self._min_dist_from_boundary = self._config["min_dist_from_boundary"] + self._max_num_samples = self._config["max_num_samples"] - self._min_theta = -np.deg2rad(self._config['delta_theta']) - self._max_theta = np.deg2rad(self._config['delta_theta']) + self._min_theta = -np.deg2rad(self._config["delta_theta"]) + self._max_theta = np.deg2rad(self._config["delta_theta"]) self._theta_rv = ss.uniform(loc=self._min_theta, scale=self._max_theta-self._min_theta) - self._min_phi = -np.deg2rad(self._config['delta_phi']) - self._max_phi = np.deg2rad(self._config['delta_phi']) + self._min_phi = -np.deg2rad(self._config["delta_phi"]) + self._max_phi = np.deg2rad(self._config["delta_phi"]) self._phi_rv = ss.uniform(loc=self._min_phi, scale=self._max_phi-self._min_phi) self._mean_depth = 0.0 - if 'mean_depth' in self._config.keys(): - self._mean_depth = self._config['mean_depth'] - self._sigma_depth = self._config['sigma_depth'] + if "mean_depth" in self._config: + self._mean_depth = self._config["mean_depth"] + self._sigma_depth = self._config["sigma_depth"] self._depth_rv = ss.norm(self._mean_depth, self._sigma_depth**2) - self._min_suction_dist = self._config['min_suction_dist'] - self._angle_dist_weight = self._config['angle_dist_weight'] - self._depth_gaussian_sigma = self._config['depth_gaussian_sigma'] + self._min_suction_dist = self._config["min_suction_dist"] + self._angle_dist_weight = self._config["angle_dist_weight"] + self._depth_gaussian_sigma = self._config["depth_gaussian_sigma"] def _sample(self, image, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D grasp candidates from a depth image. + """Sample a set of 2D grasp candidates from a depth image. Parameters ---------- - image : :obj:`perception.RgbdImage` or 'perception.DepthImage' - RGB-D or D image to sample from + image : :obj:`perception.RgbdImage` or "perception.DepthImage" + RGB-D or D image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). Returns ------- :obj:`list` of :obj:`Grasp2D` - list of 2D grasp candidates + List of 2D grasp candidates. """ if isinstance(image, RgbdImage) or isinstance(image, GdImage): depth_im = image.depth @@ -604,37 +617,36 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, else: raise ValueError("image type must be one of [RgbdImage, DepthImage, GdImage]") - # sample antipodal pairs in image space + # Sample antipodal pairs in image space. grasps = self._sample_suction_points(depth_im, camera_intr, num_samples, segmask=segmask, visualize=visualize, constraint_fn=constraint_fn) return grasps def _sample_suction_points(self, depth_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D suction point candidates from a depth image by + """Sample a set of 2D suction point candidates from a depth image by choosing points on an object surface uniformly at random - and then sampling around the surface normal + and then sampling around the surface normal. Parameters ---------- - depth_im : :obj:'perception.DepthImage' - Depth image to sample from + depth_im : :obj:"perception.DepthImage" + Depth image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). Returns ------- :obj:`list` of :obj:`SuctionPoint2D` - list of 2D suction point candidates + List of 2D suction point candidates. """ - # compute edge pixels + # Compute edge pixels. filter_start = time() if self._depth_gaussian_sigma > 0: depth_im_mask = depth_im.apply(snf.gaussian_filter, @@ -643,7 +655,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, depth_im_mask = depth_im.copy() if segmask is not None: depth_im_mask = depth_im.mask_binary(segmask) - self._logger.debug('Filtering took %.3f sec' %(time() - filter_start)) + self._logger.debug("Filtering took %.3f sec" %(time() - filter_start)) if visualize: vis.figure() @@ -653,7 +665,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, vis.imshow(depth_im_mask) vis.show() - # project to get the point cloud + # Project to get the point cloud. cloud_start = time() point_cloud_im = camera_intr.deproject_to_image(depth_im_mask) normal_cloud_im = point_cloud_im.normal_cloud_im() @@ -661,9 +673,9 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, num_nonzero_px = nonzero_px.shape[0] if num_nonzero_px == 0: return [] - self._logger.debug('Normal cloud took %.3f sec' %(time() - cloud_start)) + self._logger.debug("Normal cloud took %.3f sec" %(time() - cloud_start)) - # randomly sample points and add to image + # Randomly sample points and add to image. sample_start = time() suction_points = [] k = 0 @@ -672,36 +684,36 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, size=sample_size, replace=False) while k < sample_size and len(suction_points) < num_samples: - # sample a point uniformly at random + # Sample a point uniformly at random. ind = indices[k] center_px = np.array([nonzero_px[ind,1], nonzero_px[ind,0]]) center = Point(center_px, frame=camera_intr.frame) axis = -normal_cloud_im[center.y, center.x] depth = point_cloud_im[center.y, center.x][2] - # update number of tries + # Update number of tries. k += 1 - # check center px dist from boundary + # Check center px dist from boundary. if center_px[0] < self._min_dist_from_boundary or \ center_px[1] < self._min_dist_from_boundary or \ center_px[1] > depth_im.height - self._min_dist_from_boundary or \ center_px[0] > depth_im.width - self._min_dist_from_boundary: continue - # perturb depth + # Perturb depth. delta_depth = self._depth_rv.rvs(size=1)[0] depth = depth + delta_depth - # keep if the angle between the camera optical axis and the suction direction is less than a threshold + # Keep if the angle between the camera optical axis and the suction direction is less than a threshold. dot = max(min(axis.dot(np.array([0,0,1])), 1.0), -1.0) psi = np.arccos(dot) if psi < self._max_suction_dir_optical_axis_angle: - # create candidate grasp + # Create candidate grasp. candidate = SuctionPoint2D(center, axis, depth, camera_intr=camera_intr) - # check constraint satisfaction + # Check constraint satisfaction. if constraint_fn is None or constraint_fn(candidate): if visualize: vis.figure() @@ -710,87 +722,86 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, vis.show() suction_points.append(candidate) - self._logger.debug('Loop took %.3f sec' %(time() - sample_start)) + self._logger.debug("Loop took %.3f sec" %(time() - sample_start)) return suction_points class DepthImageMultiSuctionPointSampler(ImageGraspSampler): - """ Grasp sampler for suction points from depth images. + """Grasp sampler for suction points from depth images. Notes ----- - Required configuration parameters are specified in Other Parameters + Required configuration parameters are specified in Other Parameters. Other Parameters ---------------- max_suction_dir_optical_axis_angle : float - maximum angle, in degrees, between the suction approach axis and the camera optical axis + Maximum angle, in degrees, between the suction approach axis and the camera optical axis. delta_theta : float - maximum deviation from zero for the aziumth angle of a rotational perturbation to the surface normal (for sample diversity) + Maximum deviation from zero for the aziumth angle of a rotational perturbation to the surface normal (for sample diversity). delta_phi : float - maximum deviation from zero for the elevation angle of a rotational perturbation to the surface normal (for sample diversity) + Maximum deviation from zero for the elevation angle of a rotational perturbation to the surface normal (for sample diversity). sigma_depth : float - standard deviation for a normal distribution over depth values (for sample diversity) + Standard deviation for a normal distribution over depth values (for sample diversity). min_suction_dist : float - minimum admissible distance between suction points (for sample diversity) + Minimum admissible distance between suction points (for sample diversity). angle_dist_weight : float - amount to weight the angle difference in suction point distance computation + Amount to weight the angle difference in suction point distance computation. depth_gaussian_sigma : float - sigma used for pre-smoothing the depth image for better gradients + Sigma used for pre-smoothing the depth image for better gradients. """ def __init__(self, config): - # init superclass + # Init superclass. ImageGraspSampler.__init__(self, config) - # read params - self._max_suction_dir_optical_axis_angle = np.deg2rad(self._config['max_suction_dir_optical_axis_angle']) - self._max_dist_from_center = self._config['max_dist_from_center'] - self._min_dist_from_boundary = self._config['min_dist_from_boundary'] - self._max_num_samples = self._config['max_num_samples'] + # Read params. + self._max_suction_dir_optical_axis_angle = np.deg2rad(self._config["max_suction_dir_optical_axis_angle"]) + self._max_dist_from_center = self._config["max_dist_from_center"] + self._min_dist_from_boundary = self._config["min_dist_from_boundary"] + self._max_num_samples = self._config["max_num_samples"] - self._min_theta = -np.deg2rad(self._config['delta_theta']) - self._max_theta = np.deg2rad(self._config['delta_theta']) + self._min_theta = -np.deg2rad(self._config["delta_theta"]) + self._max_theta = np.deg2rad(self._config["delta_theta"]) self._theta_rv = ss.uniform(loc=self._min_theta, scale=self._max_theta-self._min_theta) - self._min_phi = -np.deg2rad(self._config['delta_phi']) - self._max_phi = np.deg2rad(self._config['delta_phi']) + self._min_phi = -np.deg2rad(self._config["delta_phi"]) + self._max_phi = np.deg2rad(self._config["delta_phi"]) self._phi_rv = ss.uniform(loc=self._min_phi, scale=self._max_phi-self._min_phi) self._mean_depth = 0.0 - if 'mean_depth' in self._config.keys(): - self._mean_depth = self._config['mean_depth'] - self._sigma_depth = self._config['sigma_depth'] + if "mean_depth" in self._config: + self._mean_depth = self._config["mean_depth"] + self._sigma_depth = self._config["sigma_depth"] self._depth_rv = ss.norm(self._mean_depth, self._sigma_depth**2) - self._min_suction_dist = self._config['min_suction_dist'] - self._angle_dist_weight = self._config['angle_dist_weight'] - self._depth_gaussian_sigma = self._config['depth_gaussian_sigma'] + self._min_suction_dist = self._config["min_suction_dist"] + self._angle_dist_weight = self._config["angle_dist_weight"] + self._depth_gaussian_sigma = self._config["depth_gaussian_sigma"] def _sample(self, image, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D grasp candidates from a depth image. + """Sample a set of 2D grasp candidates from a depth image. Parameters ---------- - image : :obj:`perception.RgbdImage` or 'perception.DepthImage' - RGB-D or D image to sample from + image : :obj:`perception.RgbdImage` or "perception.DepthImage" + RGB-D or D image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` - constraint function to apply to grasps + Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`Grasp2D` - list of 2D grasp candidates + List of 2D grasp candidates. """ if isinstance(image, RgbdImage) or isinstance(image, GdImage): depth_im = image.depth @@ -799,7 +810,7 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, else: raise ValueError("image type must be one of [RgbdImage, DepthImage, GdImage]") - # sample antipodal pairs in image space + # Sample antipodal pairs in image space. grasps = self._sample_suction_points(depth_im, camera_intr, num_samples, segmask=segmask, visualize=visualize, constraint_fn=constraint_fn) @@ -807,32 +818,31 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, def _sample_suction_points(self, depth_im, camera_intr, num_samples, segmask=None, visualize=False, constraint_fn=None): - """ - Sample a set of 2D suction point candidates from a depth image by + """Sample a set of 2D suction point candidates from a depth image by choosing points on an object surface uniformly at random - and then sampling around the surface normal + and then sampling around the surface normal. Parameters ---------- - depth_im : :obj:'perception.DepthImage' - Depth image to sample from + depth_im : :obj:"perception.DepthImage" + Depth image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int - number of grasps to sample + Number of grasps to sample. segmask : :obj:`perception.BinaryImage` - binary image segmenting out the object of interest + Binary image segmenting out the object of interest. visualize : bool - whether or not to show intermediate samples (for debugging) + Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` - constraint function to apply to grasps + Constraint function to apply to grasps. Returns ------- :obj:`list` of :obj:`SuctionPoint2D` - list of 2D suction point candidates + List of 2D suction point candidates. """ - # compute edge pixels + # Compute edge pixels. filter_start = time() if self._depth_gaussian_sigma > 0: depth_im_mask = depth_im.apply(snf.gaussian_filter, @@ -841,7 +851,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, depth_im_mask = depth_im.copy() if segmask is not None: depth_im_mask = depth_im.mask_binary(segmask) - self._logger.debug('Filtering took %.3f sec' %(time() - filter_start)) + self._logger.debug("Filtering took %.3f sec" %(time() - filter_start)) if visualize: vis.figure() @@ -851,7 +861,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, vis.imshow(depth_im_mask) vis.show() - # project to get the point cloud + # Project to get the point cloud. cloud_start = time() point_cloud_im = camera_intr.deproject_to_image(depth_im_mask) normal_cloud_im = point_cloud_im.normal_cloud_im() @@ -859,9 +869,9 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, num_nonzero_px = nonzero_px.shape[0] if num_nonzero_px == 0: return [] - self._logger.debug('Normal cloud took %.3f sec' %(time() - cloud_start)) + self._logger.debug("Normal cloud took %.3f sec" %(time() - cloud_start)) - # randomly sample points and add to image + # Randomly sample points and add to image. sample_start = time() suction_points = [] k = 0 @@ -870,7 +880,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, size=sample_size, replace=False) while k < sample_size and len(suction_points) < num_samples: - # sample a point uniformly at random + # Sample a point uniformly at random. ind = indices[k] center_px = np.array([nonzero_px[ind,1], nonzero_px[ind,0]]) center = Point(center_px, frame=camera_intr.frame) @@ -878,14 +888,14 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, depth = point_cloud_im[center.y, center.x][2] orientation = 2 * np.pi * np.random.rand() - # update number of tries + # Update number of tries. k += 1 - # skip bad axes + # Skip bad axes. if np.linalg.norm(axis) == 0: continue - # rotation matrix + # Rotation matrix. x_axis = axis y_axis = np.array([axis[1], -axis[0], 0]) if np.linalg.norm(y_axis) == 0: @@ -898,26 +908,26 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, t = point_cloud_im[center.y, center.x] pose = RigidTransform(rotation=R, translation=t, - from_frame='grasp', + from_frame="grasp", to_frame=camera_intr.frame) - # check center px dist from boundary + # Check center px dist from boundary. if center_px[0] < self._min_dist_from_boundary or \ center_px[1] < self._min_dist_from_boundary or \ center_px[1] > depth_im.height - self._min_dist_from_boundary or \ center_px[0] > depth_im.width - self._min_dist_from_boundary: continue - # keep if the angle between the camera optical axis and the suction direction is less than a threshold + # Keep if the angle between the camera optical axis and the suction direction is less than a threshold. dot = max(min(axis.dot(np.array([0,0,1])), 1.0), -1.0) psi = np.arccos(dot) if psi < self._max_suction_dir_optical_axis_angle: - # check distance to ensure sample diversity + # Check distance to ensure sample diversity. candidate = MultiSuctionPoint2D(pose, camera_intr=camera_intr) - # check constraint satisfaction + # Check constraint satisfaction. if constraint_fn is None or constraint_fn(candidate): if visualize: vis.figure() @@ -926,18 +936,18 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, vis.show() suction_points.append(candidate) - self._logger.debug('Loop took %.3f sec' %(time() - sample_start)) + self._logger.debug("Loop took %.3f sec" %(time() - sample_start)) return suction_points class ImageGraspSamplerFactory(object): - """ Factory for image grasp samplers. """ + """Factory for image grasp samplers.""" @staticmethod def sampler(sampler_type, config): - if sampler_type == 'antipodal_depth': + if sampler_type == "antipodal_depth": return AntipodalDepthImageGraspSampler(config) - elif sampler_type == 'suction': + elif sampler_type == "suction": return DepthImageSuctionPointSampler(config) - elif sampler_type == 'multi_suction': + elif sampler_type == "multi_suction": return DepthImageMultiSuctionPointSampler(config) else: - raise ValueError('Image grasp sampler type %s not supported!' %(sampler_type)) + raise ValueError("Image grasp sampler type %s not supported!" %(sampler_type)) diff --git a/gqcnn/grasping/policy/__init__.py b/gqcnn/grasping/policy/__init__.py index 7b6cca8e..87fa48f8 100644 --- a/gqcnn/grasping/policy/__init__.py +++ b/gqcnn/grasping/policy/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,7 +22,14 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from fc_policy import FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction -from policy import RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, PriorityCompositeGraspingPolicy, RgbdImageState, GraspAction, UniformRandomGraspingPolicy +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['FullyConvolutionalGraspingPolicyParallelJaw', 'FullyConvolutionalGraspingPolicySuction', 'RobustGraspingPolicy', 'CrossEntropyRobustGraspingPolicy', 'UniformRandomGraspingPolicy', 'RgbdImageState', 'GraspAction'] +from fc_policy import (FullyConvolutionalGraspingPolicyParallelJaw, + FullyConvolutionalGraspingPolicySuction) +from policy import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, + PriorityCompositeGraspingPolicy, RgbdImageState, + GraspAction, UniformRandomGraspingPolicy) + +__all__ = ["FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "RobustGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "UniformRandomGraspingPolicy", "RgbdImageState", "GraspAction"] diff --git a/gqcnn/grasping/policy/enums.py b/gqcnn/grasping/policy/enums.py index 5d7a5181..d9e87ed8 100644 --- a/gqcnn/grasping/policy/enums.py +++ b/gqcnn/grasping/policy/enums.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,13 +21,14 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Enums for GQ-CNN policies. +Enums for GQ-CNN policies. Author: Vishal Satish """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -class SamplingMethod: - TOP_K = 'top_k' - UNIFORM = 'uniform' +class SamplingMethod(object): + TOP_K = "top_k" + UNIFORM = "uniform" diff --git a/gqcnn/grasping/policy/fc_policy.py b/gqcnn/grasping/policy/fc_policy.py index a31d6b7f..aa128198 100644 --- a/gqcnn/grasping/policy/fc_policy.py +++ b/gqcnn/grasping/policy/fc_policy.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,82 +21,89 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Fully-Convolutional GQ-CNN grasping policies. Author: Vishal Satish """ -import math +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from abc import abstractmethod, ABCMeta +import math import os -import numpy as np import matplotlib.pyplot as plt +import numpy as np from autolab_core import Point, Logger from perception import DepthImage from visualization import Visualizer2D as vis -from gqcnn.grasping import Grasp2D, SuctionPoint2D -from gqcnn.utils import NoValidGraspsException -from enums import SamplingMethod -from policy import GraspingPolicy, GraspAction +from ...grasping import Grasp2D, SuctionPoint2D +from ...utils import NoValidGraspsException + +from .enums import SamplingMethod +from .policy import GraspingPolicy, GraspAction -class FullyConvolutionalGraspingPolicy(GraspingPolicy): - """Abstract grasp sampling policy class using Fully-Convolutional GQ-CNN network.""" - __metaclass__ = ABCMeta +class FullyConvolutionalGraspingPolicy(with_metaclass(GraspingPolicy)): + """Abstract grasp sampling policy class using Fully-Convolutional GQ-CNN + network. + """ def __init__(self, cfg, filters=None): """ Parameters ---------- cfg : dict - python dictionary of policy configuration parameters + Python dictionary of policy configuration parameters. filters : dict - python dictionary of kinematic filters to apply + Python dictionary of kinematic filters to apply. """ GraspingPolicy.__init__(self, cfg, init_sampler=False) - # init logger + # Init logger. self._logger = Logger.get_logger(self.__class__.__name__) self._cfg = cfg - self._sampling_method = self._cfg['sampling_method'] + self._sampling_method = self._cfg["sampling_method"] - # gqcnn parameters - self._gqcnn_stride = self._cfg['gqcnn_stride'] - self._gqcnn_recep_h = self._cfg['gqcnn_recep_h'] - self._gqcnn_recep_w = self._cfg['gqcnn_recep_w'] + # GQ-CNN parameters. + self._gqcnn_stride = self._cfg["gqcnn_stride"] + self._gqcnn_recep_h = self._cfg["gqcnn_recep_h"] + self._gqcnn_recep_w = self._cfg["gqcnn_recep_w"] - # grasp filtering + # Grasp filtering. self._filters = filters - self._max_grasps_to_filter = self._cfg['max_grasps_to_filter'] - self._filter_grasps = self._cfg['filter_grasps'] + self._max_grasps_to_filter = self._cfg["max_grasps_to_filter"] + self._filter_grasps = self._cfg["filter_grasps"] - # visualization parameters - self._vis_config = self._cfg['policy_vis'] - self._vis_scale = self._vis_config['scale'] - self._vis_show_axis = self._vis_config['show_axis'] + # Visualization parameters. + self._vis_config = self._cfg["policy_vis"] + self._vis_scale = self._vis_config["scale"] + self._vis_show_axis = self._vis_config["show_axis"] - self._num_vis_samples = self._vis_config['num_samples'] - self._vis_actions_2d = self._vis_config['actions_2d'] - self._vis_actions_3d = self._vis_config['actions_3d'] + self._num_vis_samples = self._vis_config["num_samples"] + self._vis_actions_2d = self._vis_config["actions_2d"] + self._vis_actions_3d = self._vis_config["actions_3d"] - self._vis_affordance_map = self._vis_config['affordance_map'] + self._vis_affordance_map = self._vis_config["affordance_map"] self._vis_output_dir = None - if 'output_dir' in self._vis_config: # if this exists in the config then all visualizations will be logged here instead of displayed - self._vis_output_dir = self._vis_config['output_dir'] + if "output_dir" in self._vis_config: # If this exists in the config then all visualizations will be logged here instead of displayed. + self._vis_output_dir = self._vis_config["output_dir"] self._state_counter = 0 def _unpack_state(self, state): - """Unpack information from the RgbdImageState""" - return state.rgbd_im.depth, state.rgbd_im.depth._data, state.segmask.raw_data, state.camera_intr #TODO: @Vishal don't access raw depth data like this + """Unpack information from the provided `RgbdImageState`.""" + return state.rgbd_im.depth, state.rgbd_im.depth._data, state.segmask.raw_data, state.camera_intr # TODO(vsatish): Don"t access raw depth data like this. def _mask_predictions(self, preds, raw_segmask): - """Mask the given predictions with the given segmask, setting the rest to 0.0.""" + """Mask the given predictions with the given segmask, setting the rest + to 0.0. + """ preds_masked = np.zeros_like(preds) - raw_segmask_cropped = raw_segmask[self._gqcnn_recep_h / 2:raw_segmask.shape[0] - self._gqcnn_recep_h / 2, self._gqcnn_recep_w / 2:raw_segmask.shape[1] - self._gqcnn_recep_w / 2, 0] + raw_segmask_cropped = raw_segmask[self._gqcnn_recep_h // 2:raw_segmask.shape[0] - self._gqcnn_recep_h // 2, self._gqcnn_recep_w // 2:raw_segmask.shape[1] - self._gqcnn_recep_w // 2, 0] raw_segmask_downsampled = raw_segmask_cropped[::self._gqcnn_stride, ::self._gqcnn_stride] if raw_segmask_downsampled.shape[0] != preds.shape[1]: raw_segmask_downsampled_new = np.zeros(preds.shape[1:3]) @@ -120,24 +130,24 @@ def _sample_predictions(self, preds, num_actions): def _sample_predictions_flat(self, preds_flat, num_samples): """Helper function to do the actual sampling.""" - if num_samples == 1: # argmax() is faster than argpartition() for special case of single sample + if num_samples == 1: # `argmax` is faster than `argpartition` for special case of single sample. if self._sampling_method == SamplingMethod.TOP_K: return [np.argmax(preds_flat)] elif self._sampling_method == SamplingMethod.UNIFORM: nonzero_ind = np.where(preds_flat > 0)[0] return np.random.choice(nonzero_ind) else: - raise ValueError('Invalid sampling method: {}'.format(self._sampling_method)) + raise ValueError("Invalid sampling method: {}".format(self._sampling_method)) else: - if self._sampling_method == 'top_k': + if self._sampling_method == "top_k": return np.argpartition(preds_flat, -1 * num_samples)[-1 * num_samples:] - elif self._sampling_method == 'uniform': + elif self._sampling_method == "uniform": nonzero_ind = np.where(preds_flat > 0)[0] if nonzero_ind.shape[0] == 0: - raise NoValidGraspsException('No grasps with nonzero quality') + raise NoValidGraspsException("No grasps with nonzero quality") return np.random.choice(nonzero_ind, size=num_samples) else: - raise ValueError('Invalid sampling method: {}'.format(self._sampling_method)) + raise ValueError("Invalid sampling method: {}".format(self._sampling_method)) @abstractmethod def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): @@ -151,21 +161,23 @@ def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, num_actions): @abstractmethod def _visualize_affordance_map(self, preds, depth_im, scale, plot_max=True, output_dir=None): - """Visualize an affordance map of the network predictions overlayed on the depth image.""" + """Visualize an affordance map of the network predictions overlayed on + the depth image. + """ pass def _visualize_2d(self, actions, preds, wrapped_depth_im, num_actions, scale, show_axis, output_dir=None): """Visualize the actions in 2D.""" - self._logger.info('Visualizing actions in 2d...') + self._logger.info("Visualizing actions in 2d...") - # plot actions in 2D + # Plot actions in 2D. vis.figure() vis.imshow(wrapped_depth_im) for i in range(num_actions): vis.grasp(actions[i].grasp, scale=scale, show_axis=show_axis, color=plt.cm.RdYlGn(actions[i].q_value)) - vis.title('Top {} Grasps'.format(num_actions)) + vis.title("Top {} Grasps".format(num_actions)) if output_dir is not None: - vis.savefig(os.path.join(output_dir, 'top_grasps.png')) + vis.savefig(os.path.join(output_dir, "top_grasps.png")) else: vis.show() @@ -173,14 +185,14 @@ def _filter(self, actions): """Filter actions.""" for action in actions: valid = True - for filter_name, is_valid in self._filters.iteritems(): + for filter_name, is_valid in self._filters.items(): if not is_valid(action.grasp): - self._logger.info('Grasp {} is not valid with filter {}'.format(action.grasp, filter_name)) + self._logger.info("Grasp {} is not valid with filter {}".format(action.grasp, filter_name)) valid = False break if valid: return action - raise NoValidGraspsException('No grasps found after filtering!') + raise NoValidGraspsException("No grasps found after filtering!") @abstractmethod def _gen_images_and_depths(self, depth, segmask): @@ -190,54 +202,54 @@ def _gen_images_and_depths(self, depth, segmask): def _action(self, state, num_actions=1): """Plan action(s).""" if self._filter_grasps: - assert self._filters is not None, 'Trying to filter grasps but no filters were provided!' - assert num_actions == 1, 'Filtering support is only implemented for single actions!' + assert self._filters is not None, "Trying to filter grasps but no filters were provided!" + assert num_actions == 1, "Filtering support is only implemented for single actions!" num_actions = self._max_grasps_to_filter - # set up log dir for state visualizations + # Set up log dir for state visualizations. state_output_dir = None if self._vis_output_dir is not None: - state_output_dir = os.path.join(self._vis_output_dir, 'state_{}'.format(str(self._state_counter).zfill(5))) + state_output_dir = os.path.join(self._vis_output_dir, "state_{}".format(str(self._state_counter).zfill(5))) if not os.path.exists(state_output_dir): os.makedirs(state_output_dir) self._state_counter += 1 - # unpack the RgbdImageState + # Unpack the `RgbdImageState`. wrapped_depth, raw_depth, raw_seg, camera_intr = self._unpack_state(state) - # predict + # Predict. images, depths = self._gen_images_and_depths(raw_depth, raw_seg) preds = self._grasp_quality_fn.quality(images, depths) - # get success probablility predictions only (this is needed because the output of the net is pairs of (p_failure, p_success)) + # Get success probablility predictions only (this is needed because the output of the network is pairs of (p_failure, p_success)). preds_success_only = preds[:, :, :, 1::2] - # mask predicted success probabilities with the cropped and downsampled object segmask so we only sample grasps on the objects + # Mask predicted success probabilities with the cropped and downsampled object segmask so we only sample grasps on the objects. preds_success_only = self._mask_predictions(preds_success_only, raw_seg) - # if we want to visualize more than one action, we have to sample more - num_actions_to_sample = self._num_vis_samples if (self._vis_actions_2d or self._vis_actions_3d) else num_actions #TODO: @Vishal if this is used with the 'top_k' sampling method, the final returned action is not the best because the argpartition does not sort the partitioned indices + # If we want to visualize more than one action, we have to sample more. + num_actions_to_sample = self._num_vis_samples if (self._vis_actions_2d or self._vis_actions_3d) else num_actions # TODO(vsatish): If this is used with the "top_k" sampling method, the final returned action is not the best because the argpartition does not sort the partitioned indices. Fix this. if self._sampling_method == SamplingMethod.TOP_K and self._num_vis_samples: - self._logger.warning('FINAL GRASP RETURNED IS NOT THE BEST!') + self._logger.warning("FINAL GRASP RETURNED IS NOT THE BEST!") - # sample num_actions_to_sample indices from the success predictions + # Sample num_actions_to_sample indices from the success predictions. sampled_ind = self._sample_predictions(preds_success_only, num_actions_to_sample) - # wrap actions to be returned + # Wrap actions to be returned. actions = self._get_actions(preds_success_only, sampled_ind, images, depths, camera_intr, num_actions_to_sample) - # filter grasps + # Filter grasps. if self._filter_grasps: actions.sort(reverse=True, key=lambda action: action.q_value) actions = [self._filter(actions)] - # visualize + # Visualize. if self._vis_actions_3d: - self._logger.info('Generating 3D Visualization...') + self._logger.info("Generating 3D Visualization...") self._visualize_3d(actions, wrapped_depth, camera_intr, num_actions_to_sample) if self._vis_actions_2d: - self._logger.info('Generating 2D visualization...') + self._logger.info("Generating 2D visualization...") self._visualize_2d(actions, preds_success_only, wrapped_depth, num_actions_to_sample, self._vis_scale, self._vis_show_axis, output_dir=state_output_dir) if self._vis_affordance_map: self._visualize_affordance_map(preds_success_only, wrapped_depth, self._vis_scale, output_dir=state_output_dir) @@ -245,19 +257,19 @@ def _action(self, state, num_actions=1): return actions[-1] if (self._filter_grasps or num_actions == 1) else actions[-(num_actions+1):] def action_set(self, state, num_actions): - """ Plan a set of actions. + """Plan a set of actions. Parameters ---------- state : :obj:`gqcnn.RgbdImageState` - the RGBD Image State + The RGBD image state. num_actions : int - the number of actions to plan + The number of actions to plan. Returns ------ list of :obj:`gqcnn.GraspAction` - the planned grasps + The planned grasps. """ return [action.grasp for action in self._action(state, num_actions=num_actions)] @@ -268,26 +280,25 @@ def __init__(self, cfg, filters=None): Parameters ---------- cfg : dict - python dictionary of policy configuration parameters + Python dictionary of policy configuration parameters. filters : dict - python dictionary of functions to apply to filter invalid grasps + Python dictionary of functions to apply to filter invalid grasps. """ FullyConvolutionalGraspingPolicy.__init__(self, cfg, filters=filters) - self._gripper_width = self._cfg['gripper_width'] + self._gripper_width = self._cfg["gripper_width"] - # depth sampling parameters - self._num_depth_bins = self._cfg['num_depth_bins'] - #TODO: ask Jeff what this is for again - self._depth_offset = 0.0 - if 'depth_offset' in self._cfg.keys(): - self._depth_offset = self._cfg['depth_offset'] + # Depth sampling parameters. + self._num_depth_bins = self._cfg["num_depth_bins"] + self._depth_offset = 0.0 # Hard offset from the workspace. + if "depth_offset" in self._cfg: + self._depth_offset = self._cfg["depth_offset"] def _sample_depths(self, raw_depth_im, raw_seg): """Sample depths from the raw depth image.""" max_depth = np.max(raw_depth_im) + self._depth_offset - # for sampling the min depth, we only sample from the portion of the depth image in the object segmask because sometimes the rim of the bin is not properly subtracted out of the depth image + # For sampling the min depth, we only sample from the portion of the depth image in the object segmask because sometimes the rim of the bin is not properly subtracted out of the depth image. raw_depth_im_segmented = np.ones_like(raw_depth_im) raw_depth_im_segmented[np.where(raw_seg > 0)] = raw_depth_im[np.where(raw_seg > 0)] min_depth = np.min(raw_depth_im_segmented) + self._depth_offset @@ -307,7 +318,7 @@ def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): h_idx = ind[i, 1] w_idx = ind[i, 2] ang_idx = ind[i, 3] - center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w / 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h / 2])) + center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2])) ang = math.pi / 2 - (ang_idx * ang_bin_width + ang_bin_width / 2) depth = depths[im_idx, 0] grasp = Grasp2D(center, ang, depth, width=self._gripper_width, camera_intr=camera_intr) @@ -326,7 +337,9 @@ def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, num_actions): raise NotImplementedError def _visualize_affordance_map(self, preds, depth_im): - """Visualize an affordance map of the network predictions overlayed on the depth image.""" + """Visualize an affordance map of the network predictions overlayed on + the depth image. + """ raise NotImplementedError class FullyConvolutionalGraspingPolicySuction(FullyConvolutionalGraspingPolicy): @@ -342,7 +355,7 @@ def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): im_idx = ind[i, 0] h_idx = ind[i, 1] w_idx = ind[i, 2] - center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w / 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h / 2])) + center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2])) axis = -normal_cloud_im[center.y, center.x] if np.linalg.norm(axis) == 0: continue @@ -355,28 +368,30 @@ def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): return actions def _visualize_affordance_map(self, preds, depth_im, scale, plot_max=True, output_dir=None): - """Visualize an affordance map of the network predictions overlayed on the depth image.""" - self._logger.info('Visualizing affordance map...') + """Visualize an affordance map of the network predictions overlayed on + the depth image. + """ + self._logger.info("Visualizing affordance map...") affordance_map = preds[0, ..., 0] tf_depth_im = depth_im.crop(depth_im.shape[0] - self._gqcnn_recep_h, depth_im.shape[1] - self._gqcnn_recep_w).resize(1.0 / self._gqcnn_stride) - # plot + # Plot. vis.figure() vis.imshow(tf_depth_im) plt.imshow(affordance_map, cmap=plt.cm.RdYlGn, alpha=0.3, vmin=0.0, vmax=1.0) if plot_max: affordance_argmax = np.unravel_index(np.argmax(affordance_map), affordance_map.shape) - plt.scatter(affordance_argmax[1], affordance_argmax[0], c='black', marker='.', s=scale*25) - vis.title('Grasp Affordance Map') + plt.scatter(affordance_argmax[1], affordance_argmax[0], c="black", marker=".", s=scale*25) + vis.title("Grasp Affordance Map") if output_dir is not None: - vis.savefig(os.path.join(output_dir, 'grasp_affordance_map.png')) + vis.savefig(os.path.join(output_dir, "grasp_affordance_map.png")) else: vis.show() def _gen_images_and_depths(self, depth, segmask): """Extend the image to a 4D tensor.""" - return np.expand_dims(depth, 0), np.array([-1]) #TODO: @Vishal depth should really be optional to the network... + return np.expand_dims(depth, 0), np.array([-1]) # TODO(vsatish): Depth should be made an optional input to the network. def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, num_actions): """Visualize the actions in 3D.""" diff --git a/gqcnn/grasping/policy/policy.py b/gqcnn/grasping/policy/policy.py index 05c31811..6bec4ed1 100644 --- a/gqcnn/grasping/policy/policy.py +++ b/gqcnn/grasping/policy/policy.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,33 +21,39 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Grasping policies + +Grasping policies. Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from abc import ABCMeta, abstractmethod -import cPickle as pkl +import copy +import pickle as pkl import math import os from time import time -import copy +from future.utils import with_metaclass +import matplotlib.pyplot as plt import numpy as np -from sklearn.mixture import GaussianMixture import scipy.ndimage.filters as snf -import matplotlib.pyplot as plt +from sklearn.mixture import GaussianMixture import autolab_core.utils as utils from autolab_core import Point, Logger -from perception import BinaryImage, ColorImage, DepthImage, RgbdImage, SegmentationImage, CameraIntrinsics +from perception import (BinaryImage, ColorImage, DepthImage, + RgbdImage, SegmentationImage, CameraIntrinsics) from visualization import Visualizer2D as vis -from gqcnn.grasping import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D, ImageGraspSamplerFactory, GraspQualityFunctionFactory, GQCnnQualityFunction, GraspConstraintFnFactory -from gqcnn.utils import GripperMode, NoValidGraspsException - -FIGSIZE = 16 -SEED = 5234709 +from .constraint_fn import GraspConstraintFnFactory +from ..grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D +from ..grasp_quality_function import (GraspQualityFunctionFactory, + GQCnnQualityFunction) +from ..image_grasp_sampler import ImageGraspSamplerFactory +from ...utils import GeneralConstants, GripperMode, NoValidGraspsException class RgbdImageState(object): """State to encapsulate RGB-D images.""" @@ -57,15 +66,15 @@ def __init__(self, rgbd_im, camera_intr, Parameters ---------- rgbd_im : :obj:`perception.RgbdImage` - an RGB-D image to plan grasps on + An RGB-D image to plan grasps on. camera_intr : :obj:`perception.CameraIntrinsics` - intrinsics of the RGB-D camera + Intrinsics of the RGB-D camera. segmask : :obj:`perception.BinaryImage` - segmentation mask for the image + Segmentation mask for the image. obj_segmask : :obj:`perception.SegmentationImage` - segmentation mask for the different objects in the image + Segmentation mask for the different objects in the image. full_observed : :obj:`object` - representation of the fully observed state + Representation of the fully observed state. """ self.rgbd_im = rgbd_im self.camera_intr = camera_intr @@ -74,21 +83,21 @@ def __init__(self, rgbd_im, camera_intr, self.fully_observed = fully_observed def save(self, save_dir): - """ Save to a directory. + """Save to a directory. Parameters ---------- save_dir : str - the directory to save to + The directory to save to. """ if not os.path.exists(save_dir): os.mkdir(save_dir) - color_image_filename = os.path.join(save_dir, 'color.png') - depth_image_filename = os.path.join(save_dir, 'depth.npy') - camera_intr_filename = os.path.join(save_dir, 'camera.intr') - segmask_filename = os.path.join(save_dir, 'segmask.npy') - obj_segmask_filename = os.path.join(save_dir, 'obj_segmask.npy') - state_filename = os.path.join(save_dir, 'state.pkl') + color_image_filename = os.path.join(save_dir, "color.png") + depth_image_filename = os.path.join(save_dir, "depth.npy") + camera_intr_filename = os.path.join(save_dir, "camera.intr") + segmask_filename = os.path.join(save_dir, "segmask.npy") + obj_segmask_filename = os.path.join(save_dir, "obj_segmask.npy") + state_filename = os.path.join(save_dir, "state.pkl") self.rgbd_im.color.save(color_image_filename) self.rgbd_im.depth.save(depth_image_filename) self.camera_intr.save(camera_intr_filename) @@ -97,25 +106,25 @@ def save(self, save_dir): if self.obj_segmask is not None: self.obj_segmask.save(obj_segmask_filename) if self.fully_observed is not None: - pkl.dump(self.fully_observed, open(state_filename, 'wb')) + pkl.dump(self.fully_observed, open(state_filename, "wb")) @staticmethod def load(save_dir): - """ Load an :obj:`RGBDImageState`. + """Load an :obj:`RGBDImageState`. Parameters ---------- save_dir : str - the directory to load from + The directory to load from. """ if not os.path.exists(save_dir): - raise ValueError('Directory %s does not exist!' %(save_dir)) - color_image_filename = os.path.join(save_dir, 'color.png') - depth_image_filename = os.path.join(save_dir, 'depth.npy') - camera_intr_filename = os.path.join(save_dir, 'camera.intr') - segmask_filename = os.path.join(save_dir, 'segmask.npy') - obj_segmask_filename = os.path.join(save_dir, 'obj_segmask.npy') - state_filename = os.path.join(save_dir, 'state.pkl') + raise ValueError("Directory %s does not exist!" %(save_dir)) + color_image_filename = os.path.join(save_dir, "color.png") + depth_image_filename = os.path.join(save_dir, "depth.npy") + camera_intr_filename = os.path.join(save_dir, "camera.intr") + segmask_filename = os.path.join(save_dir, "segmask.npy") + obj_segmask_filename = os.path.join(save_dir, "obj_segmask.npy") + state_filename = os.path.join(save_dir, "state.pkl") camera_intr = CameraIntrinsics.load(camera_intr_filename) color = ColorImage.open(color_image_filename, frame=camera_intr.frame) depth = DepthImage.open(depth_image_filename, frame=camera_intr.frame) @@ -127,7 +136,7 @@ def load(save_dir): obj_segmask = SegmentationImage.open(obj_segmask_filename, frame=camera_intr.frame) fully_observed = None if os.path.exists(state_filename): - fully_observed = pkl.load(open(state_filename, 'rb')) + fully_observed = pkl.load(open(state_filename, "rb")) return RgbdImageState(RgbdImage.from_color_and_depth(color, depth), camera_intr, segmask=segmask, @@ -135,20 +144,20 @@ def load(save_dir): fully_observed=fully_observed) class GraspAction(object): - """ Action to encapsulate grasps. + """Action to encapsulate grasps. """ def __init__(self, grasp, q_value, image=None, policy_name=None): """ Parameters ---------- grasp : :obj`Grasp2D` or :obj:`SuctionPoint2D` - 2D grasp to wrap + 2D grasp to wrap. q_value : float - grasp quality + Grasp quality. image : :obj:`perception.DepthImage` - depth image corresponding to grasp + Depth image corresponding to grasp. policy_name : str - policy name + Policy name. """ self.grasp = grasp self.q_value = q_value @@ -156,244 +165,246 @@ def __init__(self, grasp, q_value, image=None, policy_name=None): self.policy_name = policy_name def save(self, save_dir): - """ Save grasp action. + """Save grasp action. Parameters ---------- save_dir : str - directory to save the grasp action to + Directory to save the grasp action to. """ if not os.path.exists(save_dir): os.mkdir(save_dir) - grasp_filename = os.path.join(save_dir, 'grasp.pkl') - q_value_filename = os.path.join(save_dir, 'pred_robustness.pkl') - image_filename = os.path.join(save_dir, 'tf_image.npy') - pkl.dump(self.grasp, open(grasp_filename, 'wb')) - pkl.dump(self.q_value, open(q_value_filename, 'wb')) + grasp_filename = os.path.join(save_dir, "grasp.pkl") + q_value_filename = os.path.join(save_dir, "pred_robustness.pkl") + image_filename = os.path.join(save_dir, "tf_image.npy") + pkl.dump(self.grasp, open(grasp_filename, "wb")) + pkl.dump(self.q_value, open(q_value_filename, "wb")) if self.image is not None: self.image.save(image_filename) @staticmethod def load(save_dir): - """ Load a saved grasp action. + """Load a saved grasp action. Parameters ---------- save_dir : str - directory of the saved grasp action + Directory of the saved grasp action. Returns ------- :obj:`GraspAction` - loaded grasp action + Loaded grasp action. """ if not os.path.exists(save_dir): - raise ValueError('Directory %s does not exist!' %(save_dir)) - grasp_filename = os.path.join(save_dir, 'grasp.pkl') - q_value_filename = os.path.join(save_dir, 'pred_robustness.pkl') - image_filename = os.path.join(save_dir, 'tf_image.npy') - grasp = pkl.load(open(grasp_filename, 'rb')) - q_value = pkl.load(open(q_value_filename, 'rb')) + raise ValueError("Directory %s does not exist!" %(save_dir)) + grasp_filename = os.path.join(save_dir, "grasp.pkl") + q_value_filename = os.path.join(save_dir, "pred_robustness.pkl") + image_filename = os.path.join(save_dir, "tf_image.npy") + grasp = pkl.load(open(grasp_filename, "rb")) + q_value = pkl.load(open(q_value_filename, "rb")) image = None if os.path.exists(image_filename): image = DepthImage.open(image_filename) return GraspAction(grasp, q_value, image) -class Policy(object): - """ Abstract policy class. """ - __metaclass__ = ABCMeta +class Policy(with_metaclass(ABCMeta, object)): + """Abstract policy class.""" def __call__(self, state): - """ Execute the policy on a state. """ + """Execute the policy on a state.""" return self.action(state) @abstractmethod def action(self, state): - """ Returns an action for a given state. """ + """Returns an action for a given state.""" pass class GraspingPolicy(Policy): - """ Policy for robust grasping with Grasp Quality Convolutional Neural Networks (GQ-CNN). """ + """Policy for robust grasping with Grasp Quality Convolutional Neural + Networks (GQ-CNN). + """ def __init__(self, config, init_sampler=True): """ Parameters ---------- config : dict - python dictionary of parameters for the policy + Python dictionary of parameters for the policy. init_sampler : bool - whether or not to initialize the grasp sampler + Whether or not to initialize the grasp sampler. Notes ----- - Required configuration parameters are specified in Other Parameters + Required configuration parameters are specified in Other Parameters. Other Parameters ---------------- sampling : dict - dictionary of parameters for grasp sampling, see gqcnn/image_grasp_sampler.py + Dictionary of parameters for grasp sampling, see + `gqcnn/grasping/image_grasp_sampler.py`. gqcnn_model : str - string path to a trained GQ-CNN model see gqcnn/neural_networks.py + String path to a trained GQ-CNN model. """ - # store parameters + # Store parameters. self._config = config self._gripper_width = 0.05 - if 'gripper_width' in config.keys(): - self._gripper_width = config['gripper_width'] + if "gripper_width" in config: + self._gripper_width = config["gripper_width"] - # set the logging dir and possibly log file + # Set the logging dir and possibly log file. self._logging_dir = None log_file = None - if 'logging_dir' in self.config.keys(): - self._logging_dir = self.config['logging_dir'] + if "logging_dir" in self.config: + self._logging_dir = self.config["logging_dir"] if not os.path.exists(self._logging_dir): os.makedirs(self._logging_dir) - log_file = os.path.join(self._logging_dir, 'policy.log') + log_file = os.path.join(self._logging_dir, "policy.log") - # setup logger + # Setup logger. self._logger = Logger.get_logger(self.__class__.__name__, log_file=log_file, global_log_file=True) - # init grasp sampler + # Init grasp sampler. if init_sampler: - self._sampling_config = config['sampling'] - self._sampling_config['gripper_width'] = self._gripper_width - if 'crop_width' in config['metric'].keys() and 'crop_height' in config['metric'].keys(): + self._sampling_config = config["sampling"] + self._sampling_config["gripper_width"] = self._gripper_width + if "crop_width" in config["metric"] and "crop_height" in config["metric"]: pad = max( - math.ceil(np.sqrt(2) * (float(config['metric']['crop_width']) / 2)), - math.ceil(np.sqrt(2) * (float(config['metric']['crop_height']) / 2)) + math.ceil(np.sqrt(2) * (config["metric"]["crop_width"] / 2)), + math.ceil(np.sqrt(2) * (config["metric"]["crop_height"] / 2)) ) - self._sampling_config['min_dist_from_boundary'] = pad - self._sampling_config['gripper_width'] = self._gripper_width - sampler_type = self._sampling_config['type'] + self._sampling_config["min_dist_from_boundary"] = pad + self._sampling_config["gripper_width"] = self._gripper_width + sampler_type = self._sampling_config["type"] self._grasp_sampler = ImageGraspSamplerFactory.sampler(sampler_type, self._sampling_config) - # init constraint function + # Init constraint function. self._grasp_constraint_fn = None - if 'constraints' in self._config.keys(): - self._constraint_config = self._config['constraints'] - constraint_type = self._constraint_config['type'] + if "constraints" in self._config: + self._constraint_config = self._config["constraints"] + constraint_type = self._constraint_config["type"] self._grasp_constraint_fn = GraspConstraintFnFactory.constraint_fn(constraint_type, self._constraint_config) - # init grasp quality function - self._metric_config = config['metric'] - metric_type = self._metric_config['type'] + # Init grasp quality function. + self._metric_config = config["metric"] + metric_type = self._metric_config["type"] self._grasp_quality_fn = GraspQualityFunctionFactory.quality_function(metric_type, self._metric_config) @property def config(self): - """ Returns the policy configuration parameters. + """Returns the policy configuration parameters. Returns ------- dict - python dictionary of the policy configuration parameters + Python dictionary of the policy configuration parameters. """ return self._config @property def grasp_sampler(self): - """ Returns the grasp sampler. + """Returns the grasp sampler. Returns ------- :obj:`gqcnn.grasping.image_grasp_sampler.ImageGraspSampler` - the grasp sampler + The grasp sampler. """ return self._grasp_sampler @property def grasp_quality_fn(self): - """ Returns the grasp quality function. + """Returns the grasp quality function. Returns ------- :obj:`gqcnn.grasping.grasp_quality_function.GraspQualityFunction` - the grasp quality function + The grasp quality function. """ return self._grasp_quality_fn @property def grasp_constraint_fn(self): - """ Returns the grasp constraint function. + """Returns the grasp constraint function. Returns ------- :obj:`gqcnn.grasping.constraint_fn.GraspConstraintFn` - the grasp contraint function + The grasp contraint function. """ return self._grasp_constraint_fn @property def gqcnn(self): - """ Returns the GQ-CNN. + """Returns the GQ-CNN. Returns ------- :obj:`gqcnn.model.tf.GQCNNTF` - the GQ-CNN model + The GQ-CNN model. """ return self._gqcnn def set_constraint_fn(self, constraint_fn): - """ Sets the grasp constraint function. + """Sets the grasp constraint function. Parameters ---------- constraint_fn : :obj`gqcnn.grasping.constraint_fn.GraspConstraintFn` - the grasp contraint function + The grasp contraint function. """ self._grasp_constraint_fn = constraint_fn def action(self, state): - """ Returns an action for a given state. + """Returns an action for a given state. Parameters ---------- state : :obj:`RgbdImageState` - the RGB-D image state to plan grasps on + The RGB-D image state to plan grasps on. Returns ------- :obj:`GraspAction` - the planned grasp action + The planned grasp action. """ - # save state + # Save state. if self._logging_dir is not None: policy_id = utils.gen_experiment_id() - policy_dir = os.path.join(self._logging_dir, 'policy_output_%s' % (policy_id)) + policy_dir = os.path.join(self._logging_dir, "policy_output_%s" % (policy_id)) while os.path.exists(policy_dir): policy_id = utils.gen_experiment_id() - policy_dir = os.path.join(self._logging_dir, 'policy_output_%s' % (policy_id)) + policy_dir = os.path.join(self._logging_dir, "policy_output_%s" % (policy_id)) self._policy_dir = policy_dir os.mkdir(self._policy_dir) - state_dir = os.path.join(self._policy_dir, 'state') + state_dir = os.path.join(self._policy_dir, "state") state.save(state_dir) - # plan action + # Plan action. action = self._action(state) - # save action + # Save action. if self._logging_dir is not None: - action_dir = os.path.join(self._policy_dir, 'action') + action_dir = os.path.join(self._policy_dir, "action") action.save(action_dir) return action @abstractmethod def _action(self, state): - """ Returns an action for a given state. + """Returns an action for a given state. """ pass def show(self, filename=None, dpi=100): - """ Show a figure. + """Show a figure. Parameters ---------- filename : str - file to save figure to + File to save figure to. dpi : int - dpi of figure + Dpi of figure. """ if self._logging_dir is None: vis.show() @@ -402,71 +413,71 @@ def show(self, filename=None, dpi=100): vis.savefig(filename, dpi=dpi) class UniformRandomGraspingPolicy(GraspingPolicy): - """ Returns a grasp uniformly at random. """ + """Returns a grasp uniformly at random.""" def __init__(self, config): """ Parameters ---------- config : dict - python dictionary of policy configuration parameters + Python dictionary of policy configuration parameters. filters : dict - python dictionary of functions to apply to filter invalid grasps + Python dictionary of functions to apply to filter invalid grasps. """ GraspingPolicy.__init__(self, config) self._num_grasp_samples = 1 self._grasp_center_std = 0.0 - if 'grasp_center_std' in config.keys(): - self._grasp_center_std = config['grasp_center_std'] + if "grasp_center_std" in config: + self._grasp_center_std = config["grasp_center_std"] def _action(self, state): - """ Plans the grasp with the highest probability of success on + """Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` - image to plan grasps on + Image to plan grasps on. Returns ------- :obj:`GraspAction` - grasp to execute + Grasp to execute. """ - # check valid input + # Check valid input. if not isinstance(state, RgbdImageState): - raise ValueError('Must provide an RGB-D image state.') + raise ValueError("Must provide an RGB-D image state.") - # parse state + # Parse state. rgbd_im = state.rgbd_im camera_intr = state.camera_intr segmask = state.segmask - # sample grasps + # Sample grasps. grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, self._num_grasp_samples, segmask=segmask, - visualize=self.config['vis']['grasp_sampling'], + visualize=self.config["vis"]["grasp_sampling"], constraint_fn=self._grasp_constraint_fn, seed=None) num_grasps = len(grasps) if num_grasps == 0: - self._logger.warning('No valid grasps could be found') + self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - # set grasp + # Set grasp. grasp = grasps[0] - # perturb grasp + # Perturb grasp. if self._grasp_center_std > 0.0: grasp_center_rv = ss.multivariate_normal(grasp.center.data, cov=self._grasp_center_std**2) grasp.center.data = grasp_center_rv.rvs(size=1)[0] - # form tensors + # Form tensors. return GraspAction(grasp, 0.0, state.rgbd_im.depth) class RobustGraspingPolicy(GraspingPolicy): - """ Samples a set of grasp candidates in image space, + """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. """ @@ -476,164 +487,168 @@ def __init__(self, config, filters=None): Parameters ---------- config : dict - python dictionary of policy configuration parameters + Python dictionary of policy configuration parameters. filters : dict - python dictionary of functions to apply to filter invalid grasps + Python dictionary of functions to apply to filter invalid grasps. Notes ----- - Required configuration dictionary parameters are specified in Other Parameters + Required configuration dictionary parameters are specified in + Other Parameters. Other Parameters ---------------- num_grasp_samples : int - number of grasps to sample + Number of grasps to sample. gripper_width : float, optional - width of the gripper in meters + Width of the gripper in meters. logging_dir : str, optional - directory in which to save the sampled grasps and input images + Directory in which to save the sampled grasps and input images. """ GraspingPolicy.__init__(self, config) self._parse_config() self._filters = filters def _parse_config(self): - """ Parses the parameters of the policy. """ - self._num_grasp_samples = self.config['sampling']['num_grasp_samples'] + """Parses the parameters of the policy.""" + self._num_grasp_samples = self.config["sampling"]["num_grasp_samples"] self._max_grasps_filter = 1 - if 'max_grasps_filter' in self.config.keys(): - self._max_grasps_filter = self.config['max_grasps_filter'] + if "max_grasps_filter" in self.config: + self._max_grasps_filter = self.config["max_grasps_filter"] self._gripper_width = np.inf - if 'gripper_width' in self.config.keys(): - self._gripper_width = self.config['gripper_width'] + if "gripper_width" in self.config: + self._gripper_width = self.config["gripper_width"] def select(self, grasps, q_value): - """ Selects the grasp with the highest probability of success. + """Selects the grasp with the highest probability of success. + Can override for alternate policies (e.g. epsilon greedy). Parameters ---------- grasps : list - python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` grasps to select from + Python list of :obj:`gqcnn.grasping.Grasp2D` or + :obj:`gqcnn.grasping.SuctionPoint2D` grasps to select from. q_values : list - python list of associated q-values + Python list of associated q-values. Returns ------- :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` - grasp with highest probability of success + Grasp with highest probability of success . """ - # sort grasps + # Sort grasps. num_grasps = len(grasps) grasps_and_predictions = zip(np.arange(num_grasps), q_value) grasps_and_predictions.sort(key = lambda x : x[1], reverse=True) - # return top grasps + # Return top grasps. if self._filters is None: return grasps_and_predictions[0][0] - # filter grasps - self._logger.info('Filtering grasps') + # Filter grasps. + self._logger.info("Filtering grasps") i = 0 while i < self._max_grasps_filter and i < len(grasps_and_predictions): index = grasps_and_predictions[i][0] grasp = grasps[index] valid = True - for filter_name, is_valid in self._filters.iteritems(): + for filter_name, is_valid in self._filters.items(): valid = is_valid(grasp) - self._logger.debug('Grasp {} filter {} valid: {}'.format(i, filter_name, valid)) + self._logger.debug("Grasp {} filter {} valid: {}".format(i, filter_name, valid)) if not valid: valid = False break if valid: return index i += 1 - raise NoValidGraspsException('No grasps satisfied filters') + raise NoValidGraspsException("No grasps satisfied filters") def _action(self, state): - """ Plans the grasp with the highest probability of success on + """Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` - image to plan grasps on + Image to plan grasps on. Returns ------- :obj:`GraspAction` - grasp to execute + Grasp to execute. """ - # check valid input + # Check valid input. if not isinstance(state, RgbdImageState): - raise ValueError('Must provide an RGB-D image state.') + raise ValueError("Must provide an RGB-D image state.") - # parse state + # Parse state. rgbd_im = state.rgbd_im camera_intr = state.camera_intr segmask = state.segmask - # sample grasps + # Sample grasps. grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, self._num_grasp_samples, segmask=segmask, - visualize=self.config['vis']['grasp_sampling'], + visualize=self.config["vis"]["grasp_sampling"], constraint_fn=self._grasp_constraint_fn, seed=None) num_grasps = len(grasps) if num_grasps == 0: - self._logger.warning('No valid grasps could be found') + self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - # compute grasp quality + # Compute grasp quality. compute_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) - self._logger.debug('Grasp evaluation took %.3f sec' %(time()-compute_start)) + self._logger.debug("Grasp evaluation took %.3f sec" %(time()-compute_start)) - if self.config['vis']['grasp_candidates']: - # display each grasp on the original image, colored by predicted success + if self.config["vis"]["grasp_candidates"]: + # Display each grasp on the original image, colored by predicted success. norm_q_values = (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) - vis.figure(size=(FIGSIZE,FIGSIZE)) + vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, - vmin=self.config['vis']['vmin'], - vmax=self.config['vis']['vmax']) + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) for grasp, q in zip(grasps, norm_q_values): vis.grasp(grasp, scale=1.0, grasp_center_size=10, show_center=False, show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title('Sampled grasps') + vis.title("Sampled grasps") filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, 'grasp_candidates.png') + filename = os.path.join(self._logging_dir, "grasp_candidates.png") vis.show(filename) - # select grasp + # Select grasp. index = self.select(grasps, q_values) grasp = grasps[index] q_value = q_values[index] - if self.config['vis']['grasp_plan']: + if self.config["vis"]["grasp_plan"]: vis.figure() vis.imshow(rgbd_im.depth, - vmin=self.config['vis']['vmin'], - vmax=self.config['vis']['vmax']) + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) vis.grasp(grasp, scale=2.0, show_axis=True) - vis.title('Best Grasp: d=%.3f, q=%.3f' %(grasp.depth, q_value)) + vis.title("Best Grasp: d=%.3f, q=%.3f" %(grasp.depth, q_value)) vis.show() return GraspAction(grasp, q_value, state.rgbd_im.depth) class CrossEntropyRobustGraspingPolicy(GraspingPolicy): """ Optimizes a set of grasp candidates in image space using the - cross entropy method: + 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 - """ def __init__(self, config, filters=None): @@ -641,32 +656,35 @@ def __init__(self, config, filters=None): Parameters ---------- config : dict - python dictionary of policy configuration parameters + Python dictionary of policy configuration parameters. filters : dict - python dictionary of functions to apply to filter invalid grasps + Python dictionary of functions to apply to filter invalid grasps. Notes ----- - Required configuration dictionary parameters are specified in Other Parameters + Required configuration dictionary parameters are specified in Other + Parameters. Other Parameters ---------------- num_seed_samples : int - number of candidate to sample in the initial set + Number of candidate to sample in the initial set. num_gmm_samples : int - number of candidates to sample on each resampling from the GMMs + Number of candidates to sample on each resampling from the GMMs. num_iters : int - number of sample-and-refit iterations of CEM + Number of sample-and-refit iterations of CEM. gmm_refit_p : float - top p-% of grasps used for refitting + Top p-% of grasps used for refitting. gmm_component_frac : float - percentage of the elite set size used to determine number of GMM components + 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 + Regularization parameters for GMM covariance matrix, enforces + diversity of fitted distributions. deterministic : bool, optional - whether to set the random seed to enforce deterministic behavior + Whether to set the random seed to enforce deterministic behavior. gripper_width : float, optional - width of the gripper in meters + Width of the gripper in meters. """ GraspingPolicy.__init__(self, config) self._parse_config() @@ -675,125 +693,129 @@ def __init__(self, config, filters=None): self._case_counter = 0 def _parse_config(self): - """ Parses the parameters of the policy. """ - # cross entropy method parameters - self._num_seed_samples = self.config['num_seed_samples'] - self._num_gmm_samples = self.config['num_gmm_samples'] - self._num_iters = self.config['num_iters'] - self._gmm_refit_p = self.config['gmm_refit_p'] - self._gmm_component_frac = self.config['gmm_component_frac'] - self._gmm_reg_covar = self.config['gmm_reg_covar'] + """Parses the parameters of the policy.""" + # Cross entropy method parameters. + self._num_seed_samples = self.config["num_seed_samples"] + self._num_gmm_samples = self.config["num_gmm_samples"] + self._num_iters = self.config["num_iters"] + self._gmm_refit_p = self.config["gmm_refit_p"] + self._gmm_component_frac = self.config["gmm_component_frac"] + self._gmm_reg_covar = self.config["gmm_reg_covar"] self._depth_gaussian_sigma = 0.0 - if 'depth_gaussian_sigma' in self.config.keys(): - self._depth_gaussian_sigma = self.config['depth_gaussian_sigma'] + if "depth_gaussian_sigma" in self.config: + self._depth_gaussian_sigma = self.config["depth_gaussian_sigma"] self._max_grasps_filter = 1 - if 'max_grasps_filter' in self.config.keys(): - self._max_grasps_filter = self.config['max_grasps_filter'] + if "max_grasps_filter" in self.config: + self._max_grasps_filter = self.config["max_grasps_filter"] self._max_resamples_per_iteration = 100 - if 'max_resamples_per_iteration' in self.config.keys(): - self._max_resamples_per_iteration = self.config['max_resamples_per_iteration'] + if "max_resamples_per_iteration" in self.config: + self._max_resamples_per_iteration = self.config["max_resamples_per_iteration"] self._max_approach_angle = np.inf - if 'max_approach_angle' in self.config.keys(): - self._max_approach_angle = np.deg2rad(self.config['max_approach_angle']) + if "max_approach_angle" in self.config: + self._max_approach_angle = np.deg2rad(self.config["max_approach_angle"]) - # gripper parameters + # Gripper parameters. self._seed = None - if self.config['deterministic']: - self._seed = SEED + if self.config["deterministic"]: + self._seed = GeneralConstants.SEED self._gripper_width = np.inf - if 'gripper_width' in self.config.keys(): - self._gripper_width = self.config['gripper_width'] + if "gripper_width" in self.config: + self._gripper_width = self.config["gripper_width"] - # affordance map visualization + # Affordance map visualization. self._vis_grasp_affordance_map = False - if 'grasp_affordance_map' in self.config['vis'].keys(): - self._vis_grasp_affordance_map = self.config['vis']['grasp_affordance_map'] + if "grasp_affordance_map" in self.config["vis"]: + self._vis_grasp_affordance_map = self.config["vis"]["grasp_affordance_map"] - self._state_counter = 0 # used for logging state data + self._state_counter = 0 # Used for logging state data. def select(self, grasps, q_values): - """ Selects the grasp with the highest probability of success. Can override for alternate policies (e.g. epsilon greedy). + """Selects the grasp with the highest probability of success. + + Can override for alternate policies (e.g. epsilon greedy). Parameters ---------- grasps : list - python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` grasps to select from + Python list of :obj:`gqcnn.grasping.Grasp2D` or + :obj:`gqcnn.grasping.SuctionPoint2D` grasps to select from. q_values : list - python list of associated q-values + Python list of associated q-values. Returns ------- :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` - grasp with highest probability of success + Grasp with highest probability of success. """ - # sort - self._logger.info('Sorting grasps') + # Sort. + self._logger.info("Sorting grasps") num_grasps = len(grasps) if num_grasps == 0: - raise NoValidGraspsException('Zero grasps') + raise NoValidGraspsException("Zero grasps") grasps_and_predictions = zip(np.arange(num_grasps), q_values) grasps_and_predictions.sort(key = lambda x : x[1], reverse=True) - # return top grasps + # Return top grasps. if self._filters is None: return grasps_and_predictions[0][0] - # filter grasps - self._logger.info('Filtering grasps') + # Filter grasps. + self._logger.info("Filtering grasps") i = 0 while i < self._max_grasps_filter and i < len(grasps_and_predictions): index = grasps_and_predictions[i][0] grasp = grasps[index] valid = True - for filter_name, is_valid in self._filters.iteritems(): + for filter_name, is_valid in self._filters.items(): valid = is_valid(grasp) - self._logger.debug('Grasp {} filter {} valid: {}'.format(i, filter_name, valid)) + self._logger.debug("Grasp {} filter {} valid: {}".format(i, filter_name, valid)) if not valid: valid = False break if valid: return index i += 1 - raise NoValidGraspsException('No grasps satisfied filters') + raise NoValidGraspsException("No grasps satisfied filters") def _mask_predictions(self, pred_map, segmask): - self._logger.info('Masking predictions...') - assert pred_map.shape == segmask.shape, 'Prediction map shape {} does not match shape of segmask {}.'.format(pred_map.shape, segmask.shape) + self._logger.info("Masking predictions...") + assert pred_map.shape == segmask.shape, "Prediction map shape {} does not match shape of segmask {}.".format(pred_map.shape, segmask.shape) preds_masked = np.zeros_like(pred_map) nonzero_ind = np.where(segmask > 0) preds_masked[nonzero_ind] = pred_map[nonzero_ind] return preds_masked def _gen_grasp_affordance_map(self, state, stride=1): - self._logger.info('Generating grasp affordance map...') + self._logger.info("Generating grasp affordance map...") - # generate grasps at points to evaluate(this is just the interface to GraspQualityFunction) + # Generate grasps at points to evaluate (this is just the interface to + # `GraspQualityFunction`). crop_candidate_start_time = time() point_cloud_im = state.camera_intr.deproject_to_image(state.rgbd_im.depth) normal_cloud_im = point_cloud_im.normal_cloud_im() q_vals = [] - gqcnn_recep_h_half = self._grasp_quality_fn.gqcnn_recep_height / 2 - gqcnn_recep_w_half = self._grasp_quality_fn.gqcnn_recep_width / 2 + gqcnn_recep_h_half = self._grasp_quality_fn.gqcnn_recep_height // 2 + gqcnn_recep_w_half = self._grasp_quality_fn.gqcnn_recep_width // 2 im_h = state.rgbd_im.height im_w = state.rgbd_im.width for i in range(gqcnn_recep_h_half - 1, im_h - gqcnn_recep_h_half, stride): grasps = [] for j in range(gqcnn_recep_w_half - 1, im_w - gqcnn_recep_w_half, stride): - if self.config['sampling']['type'] == 'suction': #TODO: @Vishal find a better way to find policy type + if self.config["sampling"]["type"] == "suction": # TODO(vsatish): Find a better way to find policy type. grasps.append(SuctionPoint2D(Point(np.array([j, i])), axis=-normal_cloud_im[i, j], depth=state.rgbd_im.depth[i, j], camera_intr=state.camera_intr)) else: - raise NotImplementedError('Parallel Jaw Grasp Affordance Maps Not Supported!') + raise NotImplementedError("Parallel Jaw Grasp Affordance Maps Not Supported!") q_vals.extend(self._grasp_quality_fn(state, grasps)) - self._logger.info('Generating crop grasp candidates took {} sec.'.format(time() - crop_candidate_start_time)) + self._logger.info("Generating crop grasp candidates took {} sec.".format(time() - crop_candidate_start_time)) - # mask out predictions not in the segmask(we don't really care about them) - pred_map = np.array(q_vals).reshape((im_h - gqcnn_recep_h_half * 2) / stride + 1, (im_w - gqcnn_recep_w_half * 2) / stride + 1) - tf_segmask = state.segmask.crop(im_h - gqcnn_recep_h_half * 2, im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, interp='nearest')._data.squeeze() #TODO: @Vishal don't access the raw data like this! + # Mask out predictions not in the segmask (we don't really care about them). + pred_map = np.array(q_vals).reshape((im_h - gqcnn_recep_h_half * 2) // stride + 1, (im_w - gqcnn_recep_w_half * 2) // stride + 1) + tf_segmask = state.segmask.crop(im_h - gqcnn_recep_h_half * 2, im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, interp="nearest")._data.squeeze() # TODO(vsatish): Don't access the raw data like this! if tf_segmask.shape != pred_map.shape: new_tf_segmask = np.zeros_like(pred_map) smaller_i = min(pred_map.shape[0], tf_segmask.shape[0]) @@ -804,14 +826,14 @@ def _gen_grasp_affordance_map(self, state, stride=1): return pred_map_masked def _plot_grasp_affordance_map(self, state, affordance_map, stride=1, grasps=None, q_values=None, plot_max=True, title=None, scale=1.0, save_fname=None, save_path=None): - gqcnn_recep_h_half = self._grasp_quality_fn.gqcnn_recep_height / 2 - gqcnn_recep_w_half = self._grasp_quality_fn.gqcnn_recep_width / 2 + gqcnn_recep_h_half = self._grasp_quality_fn.gqcnn_recep_height // 2 + gqcnn_recep_w_half = self._grasp_quality_fn.gqcnn_recep_width // 2 im_h = state.rgbd_im.height im_w = state.rgbd_im.width - # plot + # Plot. vis.figure() - tf_depth_im = state.rgbd_im.depth.crop(im_h - gqcnn_recep_h_half * 2, im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, interp='nearest') + tf_depth_im = state.rgbd_im.depth.crop(im_h - gqcnn_recep_h_half * 2, im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, interp="nearest") vis.imshow(tf_depth_im) plt.imshow(affordance_map, cmap=plt.cm.RdYlGn, alpha=0.3) if grasps is not None: @@ -825,7 +847,7 @@ def _plot_grasp_affordance_map(self, state, affordance_map, stride=1, grasps=Non color=plt.cm.RdYlGn(q)) if plot_max: affordance_argmax = np.unravel_index(np.argmax(affordance_map), affordance_map.shape) - plt.scatter(affordance_argmax[1], affordance_argmax[0], c='black', marker='.', s=scale*25) + plt.scatter(affordance_argmax[1], affordance_argmax[0], c="black", marker=".", s=scale*25) if title is not None: vis.title(title) if save_path is not None: @@ -833,31 +855,31 @@ def _plot_grasp_affordance_map(self, state, affordance_map, stride=1, grasps=Non vis.show(save_path) def action_set(self, state): - """ Plan a set of grasps with the highest probability of success on + """Plan a set of grasps with the highest probability of success on the given RGB-D image. Parameters ---------- state : :obj:`RgbdImageState` - image to plan grasps on + Image to plan grasps on. Returns ------- python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` - grasps to execute + Grasps to execute. """ - # check valid input + # Check valid input. if not isinstance(state, RgbdImageState): - raise ValueError('Must provide an RGB-D image state.') + raise ValueError("Must provide an RGB-D image state.") state_output_dir = None if self._logging_dir is not None: - state_output_dir = os.path.join(self._logging_dir, 'state_{}'.format(str(self._state_counter).zfill(5))) + state_output_dir = os.path.join(self._logging_dir, "state_{}".format(str(self._state_counter).zfill(5))) if not os.path.exists(state_output_dir): os.makedirs(state_output_dir) self._state_counter += 1 - # parse state + # Parse state. seed_set_start = time() rgbd_im = state.rgbd_im depth_im = rgbd_im.depth @@ -872,85 +894,85 @@ def action_set(self, state): point_cloud_im = camera_intr.deproject_to_image(depth_im_filtered) normal_cloud_im = point_cloud_im.normal_cloud_im() - # vis grasp affordance map + # Visualize grasp affordance map. if self._vis_grasp_affordance_map: grasp_affordance_map = self._gen_grasp_affordance_map(state) - self._plot_grasp_affordance_map(state, grasp_affordance_map, title='Grasp Affordance Map', save_fname='affordance_map.png', save_path=state_output_dir) + self._plot_grasp_affordance_map(state, grasp_affordance_map, title="Grasp Affordance Map", save_fname="affordance_map.png", save_path=state_output_dir) - if 'input_images' in self.config['vis'].keys() and self.config['vis']['input_images']: + if "input_images" in self.config["vis"] and self.config["vis"]["input_images"]: vis.figure() vis.subplot(1,2,1) vis.imshow(depth_im) - vis.title('Depth') + vis.title("Depth") vis.subplot(1,2,2) vis.imshow(segmask) - vis.title('Segmask') + vis.title("Segmask") filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, 'input_images.png') + filename = os.path.join(self._logging_dir, "input_images.png") vis.show(filename) - # sample grasps - self._logger.info('Sampling seed set') + # Sample grasps. + self._logger.info("Sampling seed set") grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, self._num_seed_samples, segmask=segmask, - visualize=self.config['vis']['grasp_sampling'], + visualize=self.config["vis"]["grasp_sampling"], constraint_fn=self._grasp_constraint_fn, seed=self._seed) num_grasps = len(grasps) if num_grasps == 0: - self._logger.warning('No valid grasps could be found') + self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - grasp_type = 'parallel_jaw' + grasp_type = "parallel_jaw" if isinstance(grasps[0], SuctionPoint2D): - grasp_type = 'suction' + grasp_type = "suction" elif isinstance(grasps[0], MultiSuctionPoint2D): - grasp_type = 'multi_suction' + grasp_type = "multi_suction" - self._logger.info('Sampled %d grasps' %(len(grasps))) - self._logger.info('Computing the seed set took %.3f sec' %(time() - seed_set_start)) + self._logger.info("Sampled %d grasps" %(len(grasps))) + self._logger.info("Computing the seed set took %.3f sec" %(time() - seed_set_start)) - # iteratively refit and sample + # Iteratively refit and sample. for j in range(self._num_iters): - self._logger.info('CEM iter %d' %(j)) + self._logger.info("CEM iter %d" %(j)) - # predict grasps + # Predict grasps. predict_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) - self._logger.info('Prediction took %.3f sec' %(time()-predict_start)) + self._logger.info("Prediction took %.3f sec" %(time()-predict_start)) - # sort grasps + # Sort grasps. resample_start = time() q_values_and_indices = zip(q_values, np.arange(num_grasps)) q_values_and_indices.sort(key = lambda x : x[0], reverse=True) - if self.config['vis']['grasp_candidates']: - # display each grasp on the original image, colored by predicted success - norm_q_values = q_values #(q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) - title = 'Sampled Grasps Iter %d' %(j) + if self.config["vis"]["grasp_candidates"]: + # Display each grasp on the original image, colored by predicted success. + norm_q_values = q_values # (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) + title = "Sampled Grasps Iter %d" %(j) if self._vis_grasp_affordance_map: - self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=grasps, q_values=norm_q_values, scale=2.0, title=title, save_fname='cem_iter_{}.png'.format(j), save_path=state_output_dir) + self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=grasps, q_values=norm_q_values, scale=2.0, title=title, save_fname="cem_iter_{}.png".format(j), save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) display_grasps_and_q_values.sort(key = lambda x: x[1]) - vis.figure(size=(FIGSIZE,FIGSIZE)) + vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, - vmin=self.config['vis']['vmin'], - vmax=self.config['vis']['vmax']) + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) for grasp, q in display_grasps_and_q_values: vis.grasp(grasp, scale=2.0, jaw_width=2.0, show_center=False, show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title('Sampled grasps iter %d' %(j)) + vis.title("Sampled grasps iter %d" %(j)) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, 'cem_iter_%d.png' %(j)) + filename = os.path.join(self._logging_dir, "cem_iter_%d.png" %(j)) vis.show(filename) - # fit elite set + # Fit elite set. elite_start = time() num_refit = max(int(np.ceil(self._gmm_refit_p * num_grasps)), 1) elite_q_values = [i[0] for i in q_values_and_indices[:num_refit]] @@ -958,30 +980,30 @@ def action_set(self, state): elite_grasps = [grasps[i] for i in elite_grasp_indices] elite_grasp_arr = np.array([g.feature_vec for g in elite_grasps]) - if self.config['vis']['elite_grasps']: - # display each grasp on the original image, colored by predicted success + if self.config["vis"]["elite_grasps"]: + # Display each grasp on the original image, colored by predicted success. norm_q_values = (elite_q_values - np.min(elite_q_values)) / (np.max(elite_q_values) - np.min(elite_q_values)) - vis.figure(size=(FIGSIZE,FIGSIZE)) + vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, - vmin=self.config['vis']['vmin'], - vmax=self.config['vis']['vmax']) + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) for grasp, q in zip(elite_grasps, norm_q_values): vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title('Elite grasps iter %d' %(j)) + vis.title("Elite grasps iter %d" %(j)) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, 'elite_set_iter_%d.png' %(j)) + filename = os.path.join(self._logging_dir, "elite_set_iter_%d.png" %(j)) vis.show(filename) - # normalize elite set + # Normalize elite set. elite_grasp_mean = np.mean(elite_grasp_arr, axis=0) elite_grasp_std = np.std(elite_grasp_arr, axis=0) elite_grasp_std[elite_grasp_std == 0] = 1e-6 elite_grasp_arr = (elite_grasp_arr - elite_grasp_mean) / elite_grasp_std - self._logger.info('Elite set computation took %.3f sec' %(time()-elite_start)) + self._logger.info("Elite set computation took %.3f sec" %(time()-elite_start)) - # fit a GMM to the top samples + # Fit a GMM to the top samples. num_components = max(int(np.ceil(self._gmm_component_frac * num_refit)), 1) uniform_weights = (1.0 / num_components) * np.ones(num_components) gmm = GaussianMixture(n_components=num_components, @@ -989,59 +1011,59 @@ def action_set(self, state): reg_covar=self._gmm_reg_covar) train_start = time() gmm.fit(elite_grasp_arr) - self._logger.info('GMM fitting with %d components took %.3f sec' %(num_components, time()-train_start)) + self._logger.info("GMM fitting with %d components took %.3f sec" %(num_components, time()-train_start)) - # sample the next grasps + # Sample the next grasps. grasps = [] loop_start = time() num_tries = 0 while len(grasps) < self._num_gmm_samples and num_tries < self._max_resamples_per_iteration: - # sample from GMM + # Sample from GMM. sample_start = time() grasp_vecs, _ = gmm.sample(n_samples=self._num_gmm_samples) grasp_vecs = elite_grasp_std * grasp_vecs + elite_grasp_mean - self._logger.info('GMM sampling took %.3f sec' %(time()-sample_start)) + self._logger.info("GMM sampling took %.3f sec" %(time()-sample_start)) - # convert features to grasps and store if in segmask + # Convert features to grasps and store if in segmask. for k, grasp_vec in enumerate(grasp_vecs): feature_start = time() - if grasp_type == 'parallel_jaw': - # form grasp object + if grasp_type == "parallel_jaw": + # Form grasp object. grasp = Grasp2D.from_feature_vec(grasp_vec, width=self._gripper_width, camera_intr=camera_intr) - elif grasp_type == 'suction': - # read depth and approach axis + elif grasp_type == "suction": + # Read depth and approach axis. u = int(min(max(grasp_vec[1], 0), depth_im.height-1)) v = int(min(max(grasp_vec[0], 0), depth_im.width-1)) grasp_depth = depth_im[u, v, 0] - # approach_axis + # Approach axis. grasp_axis = -normal_cloud_im[u, v] - # form grasp object + # Form grasp object. grasp = SuctionPoint2D.from_feature_vec(grasp_vec, camera_intr=camera_intr, depth=grasp_depth, axis=grasp_axis) - elif grasp_type == 'multi_suction': - # read depth and approach axis + elif grasp_type == "multi_suction": + # Read depth and approach axis. u = int(min(max(grasp_vec[1], 0), depth_im.height-1)) v = int(min(max(grasp_vec[0], 0), depth_im.width-1)) grasp_depth = depth_im[u, v] - # approach_axis + # Approach_axis. grasp_axis = -normal_cloud_im[u, v] - # form grasp object + # Form grasp object. grasp = MultiSuctionPoint2D.from_feature_vec(grasp_vec, camera_intr=camera_intr, depth=grasp_depth, axis=grasp_axis) - self._logger.debug('Feature vec took %.5f sec' %(time()-feature_start)) + self._logger.debug("Feature vec took %.5f sec" %(time()-feature_start)) bounds_start = time() - # check in bounds + # Check in bounds. if state.segmask is None or \ (grasp.center.y >= 0 and grasp.center.y < state.segmask.height and \ grasp.center.x >= 0 and grasp.center.x < state.segmask.width and \ @@ -1049,139 +1071,141 @@ def action_set(self, state): grasp.approach_angle < self._max_approach_angle) and \ (self._grasp_constraint_fn is None or self._grasp_constraint_fn(grasp)): - # check validity according to filters + # Check validity according to filters. grasps.append(grasp) - self._logger.debug('Bounds took %.5f sec' %(time()-bounds_start)) + self._logger.debug("Bounds took %.5f sec" %(time()-bounds_start)) num_tries += 1 - # check num grasps + # Check num grasps. num_grasps = len(grasps) if num_grasps == 0: - self._logger.warning('No valid grasps could be found') + self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - self._logger.info('Resample loop took %.3f sec' %(time()-loop_start)) - self._logger.info('Resampling took %.3f sec' %(time()-resample_start)) + self._logger.info("Resample loop took %.3f sec" %(time()-loop_start)) + self._logger.info("Resampling took %.3f sec" %(time()-resample_start)) - # predict final set of grasps + # Predict final set of grasps. predict_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) - self._logger.info('Final prediction took %.3f sec' %(time()-predict_start)) + self._logger.info("Final prediction took %.3f sec" %(time()-predict_start)) - if self.config['vis']['grasp_candidates']: - # display each grasp on the original image, colored by predicted success - norm_q_values = q_values #(q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) - title = 'Final Sampled Grasps' + if self.config["vis"]["grasp_candidates"]: + # Display each grasp on the original image, colored by predicted success. + norm_q_values = q_values # (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) + title = "Final Sampled Grasps" if self._vis_grasp_affordance_map: - self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=grasps, q_values=norm_q_values, scale=2.0, title=title, save_fname='final_sampled_grasps.png'.format(j), save_path=state_output_dir) + self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=grasps, q_values=norm_q_values, scale=2.0, title=title, save_fname="final_sampled_grasps.png".format(j), save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) display_grasps_and_q_values.sort(key = lambda x: x[1]) - vis.figure(size=(FIGSIZE,FIGSIZE)) + vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, - vmin=self.config['vis']['vmin'], - vmax=self.config['vis']['vmax']) + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) for grasp, q in display_grasps_and_q_values: vis.grasp(grasp, scale=2.0, jaw_width=2.0, show_center=False, show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title('Sampled grasps iter %d' %(j)) + vis.title("Sampled grasps iter %d" %(j)) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, 'cem_iter_%d.png' %(j)) + filename = os.path.join(self._logging_dir, "cem_iter_%d.png" %(j)) vis.show(filename) return grasps, q_values def _action(self, state): - """ Plans the grasp with the highest probability of success on + """Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` - image to plan grasps on + Image to plan grasps on. Returns ------- :obj:`GraspAction` - grasp to execute + Grasp to execute. """ - # parse state + # Parse state. rgbd_im = state.rgbd_im depth_im = rgbd_im.depth camera_intr = state.camera_intr segmask = state.segmask - # plan grasps + # Plan grasps. grasps, q_values = self.action_set(state) - # select grasp + # Select grasp. index = self.select(grasps, q_values) grasp = grasps[index] q_value = q_values[index] - if self.config['vis']['grasp_plan']: - title = 'Best Grasp: d=%.3f, q=%.3f' %(grasp.depth, q_value) + if self.config["vis"]["grasp_plan"]: + title = "Best Grasp: d=%.3f, q=%.3f" %(grasp.depth, q_value) if self._vis_grasp_affordance_map: - self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=[grasp], q_values=[q_value], scale=2.0, title=title, save_fname=os.path.join(case_output_dir, 'best_grasp.png')) + self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=[grasp], q_values=[q_value], scale=2.0, title=title, save_fname=os.path.join(case_output_dir, "best_grasp.png")) else: vis.figure() vis.imshow(rgbd_im.depth, - vmin=self.config['vis']['vmin'], - vmax=self.config['vis']['vmax']) + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) vis.grasp(grasp, scale=5.0, show_center=False, show_axis=True, jaw_width=1.0, grasp_axis_width=0.2) vis.title(title) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, 'planned_grasp.png') + filename = os.path.join(self._logging_dir, "planned_grasp.png") vis.show(filename) - # form return image + # Form return image. image = state.rgbd_im.depth if isinstance(self._grasp_quality_fn, GQCnnQualityFunction): image_arr, _ = self._grasp_quality_fn.grasps_to_tensors([grasp], state) image = DepthImage(image_arr[0,...], frame=state.rgbd_im.frame) - # return action + # Return action. action = GraspAction(grasp, q_value, image) return action class QFunctionRobustGraspingPolicy(CrossEntropyRobustGraspingPolicy): - """ Optimizes a set of antipodal grasp candidates in image space using the + """Optimizes a set of antipodal grasp candidates in image space using the cross entropy method with a GQ-CNN that estimates the Q-function for use in Q-learning. Notes ----- - Required configuration parameters are specified in Other Parameters + Required configuration parameters are specified in Other Parameters. Other Parameters ---------------- reinit_pc1 : bool - whether or not to reinitialize the pc1 layer of the GQ-CNN + Whether or not to reinitialize the pc1 layer of the GQ-CNN. reinit_fc3: bool - whether or not to reinitialize the fc3 layer of the GQ-CNN + Whether or not to reinitialize the fc3 layer of the GQ-CNN. reinit_fc4: bool - whether or not to reinitialize the fc4 layer of the GQ-CNN + Whether or not to reinitialize the fc4 layer of the GQ-CNN. reinit_fc5: bool - whether or not to reinitialize the fc5 layer of the GQ-CNN + Whether or not to reinitialize the fc5 layer of the GQ-CNN. num_seed_samples : int - number of candidate to sample in the initial set + Number of candidate to sample in the initial set. num_gmm_samples : int - number of candidates to sample on each resampling from the GMMs + Number of candidates to sample on each resampling from the GMMs. num_iters : int - number of sample-and-refit iterations of CEM + Number of sample-and-refit iterations of CEM. gmm_refit_p : float - top p-% of grasps used for refitting + Top p-% of grasps used for refitting. gmm_component_frac : float - percentage of the elite set size used to determine number of GMM components + 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 + Regularization parameters for GMM covariance matrix, enforces diversity + of fitted distributions. deterministic : bool, optional - whether to set the random seed to enforce deterministic behavior + Whether to set the random seed to enforce deterministic behavior. gripper_width : float, optional - width of the gripper in meters + Width of the gripper in meters. """ def __init__(self, config): CrossEntropyRobustGraspingPolicy.__init__(self, config) @@ -1189,40 +1213,40 @@ def __init__(self, config): self._setup_gqcnn() def _parse_config(self): - """ Parses the parameters of the policy. """ - self._reinit_pc1 = self.config['reinit_pc1'] - self._reinit_fc3 = self.config['reinit_fc3'] - self._reinit_fc4 = self.config['reinit_fc4'] - self._reinit_fc5 = self.config['reinit_fc5'] + """Parses the parameters of the policy.""" + self._reinit_pc1 = self.config["reinit_pc1"] + self._reinit_fc3 = self.config["reinit_fc3"] + self._reinit_fc4 = self.config["reinit_fc4"] + self._reinit_fc5 = self.config["reinit_fc5"] def _setup_gqcnn(self): - """ Sets up the GQ-CNN. """ - # close existing session (from superclass initializer) + """Sets up the GQ-CNN.""" + # Close existing session (from superclass initializer). self.gqcnn.close_session() - # check valid output size + # Check valid output size. if self.gqcnn.fc5_out_size != 1 and not self._reinit_fc5: - raise ValueError('Q function must return scalar values') + raise ValueError("Q function must return scalar values") - # reinitialize layers + # Reinitialize layers if self._reinit_fc5: self.gqcnn.fc5_out_size = 1 - # TODO: implement reinitialization of pc0 + # TODO(jmahler): Implement reinitialization of pc0. self.gqcnn.reinitialize_layers(self._reinit_fc3, self._reinit_fc4, self._reinit_fc5) self.gqcnn.initialize_network(add_softmax=False) class EpsilonGreedyQFunctionRobustGraspingPolicy(QFunctionRobustGraspingPolicy): - """ Optimizes a set of antipodal grasp candidates in image space + """Optimizes a set of antipodal grasp candidates in image space using the cross entropy method with a GQ-CNN that estimates the Q-function for use in Q-learning, and chooses a random antipodal grasp with probability epsilon. Notes ----- - Required configuration parameters are specified in Other Parameters + Required configuration parameters are specified in Other Parameters. Other Parameters ---------------- @@ -1233,8 +1257,8 @@ def __init__(self, config): self._parse_config() def _parse_config(self): - """ Parses the parameters of the policy. """ - self._epsilon = self.config['epsilon'] + """Parses the parameters of the policy.""" + self._epsilon = self.config["epsilon"] @property def epsilon(self): @@ -1245,81 +1269,81 @@ def epsilon(self, val): self._epsilon = val def greedy_action(self, state): - """ Plans the grasp with the highest probability of success on + """Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` - image to plan grasps on + Image to plan grasps on. Returns ------- :obj:`GraspAction` - grasp to execute + Grasp to execute. """ return CrossEntropyRobustGraspingPolicy.action(self, state) def _action(self, state): - """ Plans the grasp with the highest probability of success on + """Plans the grasp with the highest probability of success on the given RGB-D image. Attributes ---------- state : :obj:`RgbdImageState` - image to plan grasps on + Image to plan grasps on. Returns ------- :obj:`GraspAction` - grasp to execute + Grasp to execute. """ - # take the greedy action with prob 1 - epsilon + # Take the greedy action with prob 1-epsilon. if np.random.rand() > self.epsilon: - self._logger.debug('Taking greedy action') + self._logger.debug("Taking greedy action") return CrossEntropyRobustGraspingPolicy.action(self, state) - # otherwise take a random action - self._logger.debug('Taking random action') + # Otherwise take a random action. + self._logger.debug("Taking random action") - # check valid input + # Check valid input. if not isinstance(state, RgbdImageState): - raise ValueError('Must provide an RGB-D image state.') + raise ValueError("Must provide an RGB-D image state.") - # parse state + # Parse state. rgbd_im = state.rgbd_im camera_intr = state.camera_intr segmask = state.segmask - # sample random antipodal grasps + # Sample random antipodal grasps. grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, self._num_seed_samples, segmask=segmask, - visualize=self.config['vis']['grasp_sampling'], + visualize=self.config["vis"]["grasp_sampling"], constraint_fn=self._grasp_constraint_fn, seed=self._seed) num_grasps = len(grasps) if num_grasps == 0: - self._logger.warning('No valid grasps could be found') + self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - # choose a grasp uniformly at random + # Choose a grasp uniformly at random. grasp_ind = np.random.choice(num_grasps, size=1)[0] grasp = grasps[grasp_ind] depth = grasp.depth - # create transformed image + # Create transformed image. image_tensor, pose_tensor = self.grasps_to_tensors([grasp], state) image = DepthImage(image_tensor[0,...]) - # predict prob success + # Predict prob success. output_arr = self.gqcnn.predict(image_tensor, pose_tensor) q_value = output_arr[0,-1] - # visualize planned grasp - if self.config['vis']['grasp_plan']: - scale_factor = float(self.gqcnn.im_width) / float(self._crop_width) + # Visualize planned grasp. + if self.config["vis"]["grasp_plan"]: + scale_factor = self.gqcnn.im_width / self._crop_width scaled_camera_intr = camera_intr.resize(scale_factor) vis_grasp = Grasp2D(Point(image.center), 0.0, depth, width=self._gripper_width, @@ -1327,19 +1351,19 @@ def _action(self, state): vis.figure() vis.imshow(image) vis.grasp(vis_grasp, scale=1.5, show_center=False, show_axis=True) - vis.title('Best Grasp: d=%.3f, q=%.3f' %(depth, q_value)) + vis.title("Best Grasp: d=%.3f, q=%.3f" %(depth, q_value)) vis.show() - # return action + # Return action. return GraspAction(grasp, q_value, image) class CompositeGraspingPolicy(Policy): - """Grasping policy composed of multiple sub-policies + """Grasping policy composed of multiple sub-policies. Attributes ---------- policies : dict mapping str to `gqcnn.GraspingPolicy` - key-value dict mapping policy names to grasping policies + Key-value dict mapping policy names to grasping policies. """ def __init__(self, policies): self._policies = policies @@ -1358,10 +1382,10 @@ def set_constraint_fn(self, constraint_fn): class PriorityCompositeGraspingPolicy(CompositeGraspingPolicy): def __init__(self, policies, priority_list): - # check validity + # Check validity. for name in priority_list: - if str(name) not in policies.keys(): - raise ValueError('Policy named %s is not in the list of policies!' %(name)) + if str(name) not in policies: + raise ValueError("Policy named %s is not in the list of policies!" %(name)) self._priority_list = priority_list CompositeGraspingPolicy.__init__(self, policies) @@ -1371,7 +1395,7 @@ def priority_list(self): return self._priority_list def action(self, state, policy_subset=None, min_q_value=-1.0): - """ Returns an action for a given state. + """Returns an action for a given state. """ action = None i = 0 @@ -1382,7 +1406,7 @@ def action(self, state, policy_subset=None, min_q_value=-1.0): if policy_subset is not None and name not in policy_subset: i += 1 continue - self._logger.info('Planning action for sub-policy {}'.format(name)) + self._logger.info("Planning action for sub-policy {}".format(name)) try: action = self.policies[policy_name].action(state) action.policy_name = name @@ -1395,7 +1419,7 @@ def action(self, state, policy_subset=None, min_q_value=-1.0): return action def action_set(self, state, policy_subset=None, min_q_value=-1.0): - """ Returns an action for a given state. + """Returns an action for a given state. """ actions = None q_values = None @@ -1406,7 +1430,7 @@ def action_set(self, state, policy_subset=None, min_q_value=-1.0): if policy_subset is not None and name not in policy_subset: i += 1 continue - self._logger.info('Planning action set for sub-policy {}'.format(name)) + self._logger.info("Planning action set for sub-policy {}".format(name)) try: actions, q_values = self.policies[name].action_set(state) for action in actions: @@ -1424,11 +1448,11 @@ def __init__(self, policies): CompositeGraspingPolicy.__init__(self, policies) def action(self, state, policy_subset=None, min_q_value=-1.0): - """ Returns an action for a given state. + """Returns an action for a given state. """ - # compute all possible actions + # Compute all possible actions. actions = [] - for name, policy in self.policies.iteritems(): + for name, policy in self.policies.items(): if policy_subset is not None and name not in policy_subset: continue try: @@ -1441,16 +1465,16 @@ def action(self, state, policy_subset=None, min_q_value=-1.0): if len(actions) == 0: raise NoValidGraspsException() - # rank based on q value + # Rank based on q value. actions.sort(key = lambda x: x.q_value, reverse=True) return actions[0] def action_set(self, state, policy_subset=None, min_q_value=-1.0): - """ Returns an action for a given state. + """Returns an action for a given state. """ actions = [] q_values = [] - for name, policy in self.policies.iteritems(): + for name, policy in self.policies.items(): if policy_subset is not None and name not in policy_subset: continue try: @@ -1464,4 +1488,3 @@ def action_set(self, state, policy_subset=None, min_q_value=-1.0): if actions is None: raise NoValidGraspsException() return actions, q_values - diff --git a/gqcnn/model/__init__.py b/gqcnn/model/__init__.py index 4c21d923..52fa9b96 100644 --- a/gqcnn/model/__init__.py +++ b/gqcnn/model/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,26 +21,29 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Factory functions to obtain GQCNN/FCGQCNN class based on backend. -Author ------- -Vishal Satish +Factory functions to obtain `GQCNN`/`FCGQCNN` class based on backend. +Author: Vishal Satish """ -from tf import * +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +from .tf import * from autolab_core import Logger -def get_gqcnn_model(backend='tf', verbose=True): +def get_gqcnn_model(backend="tf", verbose=True): """ - Get the GQ-CNN model for the provided backend. Currently only TensorFlow is supported. + Get the GQ-CNN model for the provided backend. + + Note: + Currently only TensorFlow is supported. Parameters ---------- backend : str - the backend to use, currently only 'tf' is supported + the backend to use, currently only "tf" is supported verbose : bool whether or not to log initialization output to stdout @@ -47,24 +53,27 @@ def get_gqcnn_model(backend='tf', verbose=True): GQ-CNN model with TensorFlow backend """ - # set up logger - logger = Logger.get_logger('GQCNNModelFactory', silence=(not verbose)) + # Set up logger. + logger = Logger.get_logger("GQCNNModelFactory", silence=(not verbose)) - # return desired GQ-CNN instance based on backend - if backend == 'tf': - logger.info('Initializing GQ-CNN with Tensorflow as backend...') + # Return desired GQ-CNN instance based on backend. + if backend == "tf": + logger.info("Initializing GQ-CNN with Tensorflow as backend...") return GQCNNTF else: - raise ValueError('Invalid backend: {}'.format(backend)) + raise ValueError("Invalid backend: {}".format(backend)) -def get_fc_gqcnn_model(backend='tf', verbose=True): +def get_fc_gqcnn_model(backend="tf", verbose=True): """ - Get the FC-GQ-CNN model for the provided backend. Currently only TensorFlow is supported. + Get the FC-GQ-CNN model for the provided backend. + + Note: + Currently only TensorFlow is supported. Parameters ---------- backend : str - the backend to use, currently only 'tf' is supported + the backend to use, currently only "tf" is supported verbose : bool whether or not to log initialization output to stdout @@ -74,12 +83,12 @@ def get_fc_gqcnn_model(backend='tf', verbose=True): FC-GQ-CNN model with TensorFlow backend """ - # set up logger - logger = Logger.get_logger('FCGQCNNModelFactory', silence=(not verbose)) + # Set up logger. + logger = Logger.get_logger("FCGQCNNModelFactory", silence=(not verbose)) - # return desired Fully-Convolutional GQ-CNN instance based on backend - if backend == 'tf': - logger.info('Initializing FC-GQ-CNN with Tensorflow as backend...') + # Return desired Fully-Convolutional GQ-CNN instance based on backend. + if backend == "tf": + logger.info("Initializing FC-GQ-CNN with Tensorflow as backend...") return FCGQCNNTF else: - raise ValueError('Invalid backend: {}'.format(backend)) + raise ValueError("Invalid backend: {}".format(backend)) diff --git a/gqcnn/model/tf/__init__.py b/gqcnn/model/tf/__init__.py index 216556b8..a9c62a86 100644 --- a/gqcnn/model/tf/__init__.py +++ b/gqcnn/model/tf/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,7 +22,11 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from network_tf import GQCNNTF -from fc_network_tf import FCGQCNNTF +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['GQCNNTF', 'FCGQCNNTF'] +from .network_tf import GQCNNTF +from .fc_network_tf import FCGQCNNTF + +__all__ = ["GQCNNTF", "FCGQCNNTF"] diff --git a/gqcnn/model/tf/fc_network_tf.py b/gqcnn/model/tf/fc_network_tf.py index ff3b60e3..f2693a9e 100644 --- a/gqcnn/model/tf/fc_network_tf.py +++ b/gqcnn/model/tf/fc_network_tf.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,30 +21,39 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + FC-GQ-CNN network implemented in Tensorflow Author: Vishal Satish """ -import os -import json +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from collections import OrderedDict +import json +import os import sys import tensorflow as tf -from network_tf import GQCNNTF -from gqcnn.utils import TrainingMode, InputDepthMode +from .network_tf import GQCNNTF +from ...utils import TrainingMode, InputDepthMode class FCGQCNNTF(GQCNNTF): - """FC-GQ-CNN network implemented in Tensorflow. Note that FC-GQ-CNNs are never directly trained, but instead a pre-trained GQ-CNN is converted to an FC-GQ-CNN at inference time.""" + """FC-GQ-CNN network implemented in Tensorflow. + + Note: + FC-GQ-CNNs are never directly trained, but instead a pre-trained GQ-CNN + is converted to an FC-GQ-CNN at inference time. + """ def __init__(self, gqcnn_config, fc_config, verbose=True, log_file=None): """ Parameters ---------- gqcnn_config : dict - python dictionary of pre-trained GQ-CNN model configuration parameters + python dictionary of pre-trained GQ-CNN model configuration + parameters fc_config : dict python dictionary of FC-GQ-CNN model configuration parameters verbose : bool @@ -49,15 +61,14 @@ def __init__(self, gqcnn_config, fc_config, verbose=True, log_file=None): log_file : str if provided, model output will also be logged to this file """ - super(FCGQCNNTF, self).__init__(gqcnn_config, log_file=log_file) super(FCGQCNNTF, self)._parse_config(gqcnn_config) - self._parse_config(fc_config) # we call this again(even though it gets called in the parent constructor on line 42) because the call to the parent _parse_config() on line 43 overwrites our first call + self._parse_config(fc_config) # We call this again (even though it gets called in the parent `GQCNNTF` constructor) because the call to the parent `_parse_config` overwrites the first call. - # check that conv layers of GQ-CNN were trained with VALID padding - for layer_name, layer_config in self._architecture['im_stream'].iteritems(): - if layer_config['type'] == 'conv': - assert layer_config['pad'] == 'VALID', 'GQ-CNN used for FC-GQ-CNN must have VALID padding for conv layers. Found layer: {} with padding: {}'.format(layer_name, layer_config['pad']) + # Check that conv layers of GQ-CNN were trained with VALID padding. + for layer_name, layer_config in self._architecture["im_stream"].items(): + if layer_config["type"] == "conv": + assert layer_config["pad"] == "VALID", "GQ-CNN used for FC-GQ-CNN must have VALID padding for conv layers. Found layer: {} with padding: {}".format(layer_name, layer_config["pad"]) @staticmethod def load(model_dir, fc_config, log_file=None): @@ -77,108 +88,114 @@ def load(model_dir, fc_config, log_file=None): :obj:`FCGQCNNTF` initialized FC-GQ-CNN """ - config_file = os.path.join(model_dir, 'config.json') + config_file = os.path.join(model_dir, "config.json") with open(config_file) as data_file: train_config = json.load(data_file, object_pairs_hook=OrderedDict) - gqcnn_config = train_config['gqcnn'] + gqcnn_config = train_config["gqcnn"] - # initialize weights and Tensorflow network + # Initialize weights and Tensorflow network. fcgqcnn = FCGQCNNTF(gqcnn_config, fc_config, log_file=log_file) - fcgqcnn.init_weights_file(os.path.join(model_dir, 'model.ckpt')) + fcgqcnn.init_weights_file(os.path.join(model_dir, "model.ckpt")) fcgqcnn.init_mean_and_std(model_dir) - training_mode = train_config['training_mode'] + training_mode = train_config["training_mode"] if training_mode == TrainingMode.CLASSIFICATION: fcgqcnn.initialize_network(add_softmax=True) elif training_mode == TrainingMode.REGRESSION: fcgqcnn.initialize_network() else: - raise ValueError('Invalid training mode: {}'.format(training_mode)) + raise ValueError("Invalid training mode: {}".format(training_mode)) return fcgqcnn def _parse_config(self, cfg): - # override GQ-CNN image height and width - self._im_width = cfg['im_width'] - self._im_height = cfg['im_height'] + # Override GQ-CNN image height and width. + self._im_width = cfg["im_width"] + self._im_height = cfg["im_height"] def _pack(self, dim_h, dim_w, data, vector=False): if vector: - # first reshape vector into 3-dimensional tensor + # First reshape vector into 3-dimensional tensor. reshaped = tf.reshape(data, tf.concat([[1, 1], tf.shape(data)], 0)) - # then tile into tensor of shape dim x dim x data.dim0 + # Then tile into tensor of shape dim_h x dim_w x `data.dim0`. packed = tf.tile(reshaped, [dim_h, dim_w, 1]) else: - # first reshape second dimension of tensor into 3-dimensional tensor + # First reshape second dimension of tensor into 3-dimensional tensor. reshaped = tf.reshape(data, tf.concat([tf.shape(data)[0:1], [1, 1], tf.shape(data)[1:]], 0)) - # then tile into tensor of shape bsize x dim_h x dim_w x data.dim1 + # Then tile into tensor of shape bsize=1 x dim_h x dim_w x `data.dim1`. packed = tf.tile(reshaped, [1, dim_h, dim_w, 1]) return packed def _build_fully_conv_layer(self, input_node, filter_dim, fc_name, final_fc_layer=False): - self._logger.info('Converting fc layer {} to fully convolutional...'.format(fc_name)) + self._logger.info("Converting fc layer {} to fully convolutional...".format(fc_name)) - # create new set of weights by reshaping fully connected layer weights - fcW = self._weights.weights['{}_weights'.format(fc_name)] - convW = tf.Variable(tf.reshape(fcW, tf.concat([[filter_dim, filter_dim], [tf.shape(fcW)[0] / (filter_dim * filter_dim)], tf.shape(fcW)[1:]], 0)), name='{}_fully_conv_weights'.format(fc_name)) - self._weights.weights['{}_fully_conv_weights'.format(fc_name)] = convW + # Create new set of weights by reshaping fully connected layer weights. + fcW = self._weights.weights["{}_weights".format(fc_name)] + convW = tf.Variable(tf.reshape(fcW, tf.concat([[filter_dim, filter_dim], [tf.shape(fcW)[0] // (filter_dim * filter_dim)], tf.shape(fcW)[1:]], 0)), name="{}_fully_conv_weights".format(fc_name)) + self._weights.weights["{}_fully_conv_weights".format(fc_name)] = convW - # get bias - convb = self._weights.weights['{}_bias'.format(fc_name)] + # Get bias. + convb = self._weights.weights["{}_bias".format(fc_name)] - # compute conv out(note that we use padding='VALID' here because we want an output size of 1x1xnum_filts for the original input size) - convh = tf.nn.conv2d(input_node, convW, strides=[1, 1, 1, 1], padding='VALID') + # Compute conv out (note that we use padding="VALID" here because we want an output size of 1 x 1 x num_filts for the original input size). + convh = tf.nn.conv2d(input_node, convW, strides=[1, 1, 1, 1], padding="VALID") - # pack bias into tensor of shape=tf.shape(convh) + # Pack bias into tensor of shape `tf.shape(convh)`. + # TODO(vsatish): This is unnecessary and can be removed for potential + # performance improvements. bias_packed = self._pack(tf.shape(convh)[1], tf.shape(convh)[2], convb, vector=True) - # add bias term + # Add bias term. convh = convh + bias_packed - # apply activation + # Apply activation. if not final_fc_layer: convh = self._leaky_relu(convh, alpha=self._relu_coeff) - # add output to feature_dict + # Add output to feature_dict. self._feature_tensors[fc_name] = convh return convh def _build_fully_conv_merge_layer(self, input_node_im, input_node_pose, filter_dim, fc_name): - self._logger.info('Converting fc merge layer {} to fully convolutional...'.format(fc_name)) + self._logger.info("Converting fc merge layer {} to fully convolutional...".format(fc_name)) - # create new set of weights for image stream by reshaping fully-connected layer weights - fcW_im = self._weights.weights['{}_input_1_weights'.format(fc_name)] - convW = tf.Variable(tf.reshape(fcW_im, tf.concat([[filter_dim, filter_dim], [tf.shape(fcW_im)[0] / (filter_dim * filter_dim)], tf.shape(fcW_im)[1:]], 0)), name='{}_im_fully_conv_weights'.format(fc_name)) - self._weights.weights['{}_im_fully_conv_weights'.format(fc_name)] = convW + # Create new set of weights for image stream by reshaping fully-connected layer weights. + fcW_im = self._weights.weights["{}_input_1_weights".format(fc_name)] + convW = tf.Variable(tf.reshape(fcW_im, tf.concat([[filter_dim, filter_dim], [tf.shape(fcW_im)[0] // (filter_dim * filter_dim)], tf.shape(fcW_im)[1:]], 0)), name="{}_im_fully_conv_weights".format(fc_name)) + self._weights.weights["{}_im_fully_conv_weights".format(fc_name)] = convW - # compute im stream conv out(note that we use padding='VALID' here because we want an output size of 1x1xnum_filts for the original input size) - convh_im = tf.nn.conv2d(input_node_im, convW, strides=[1, 1, 1, 1], padding='VALID') + # Compute im stream conv out (note that we use padding="VALID" here because we want an output size of 1 x 1 x num_filts for the original input size). + convh_im = tf.nn.conv2d(input_node_im, convW, strides=[1, 1, 1, 1], padding="VALID") - # get pose stream fully-connected weights - fcW_pose = self._weights.weights['{}_input_2_weights'.format(fc_name)] + # Get pose stream fully-connected weights. + fcW_pose = self._weights.weights["{}_input_2_weights".format(fc_name)] - # compute matmul for pose stream + # Compute matmul for pose stream. pose_out = tf.matmul(input_node_pose, fcW_pose) - # pack pose_out into a tensor of shape=tf.shape(convh_im) + # Pack pose_out into a tensor of shape=tf.shape(convh_im). + # TODO(vsatish): This is unnecessary and can be removed for potential + # performance improvements. pose_packed = self._pack(tf.shape(convh_im)[1], tf.shape(convh_im)[2], pose_out) - # add the im and pose tensors + # Add the im and pose tensors. convh = convh_im + pose_packed - # pack bias - fc_bias = self._weights.weights['{}_bias'.format(fc_name)] + # Pack bias. + # TODO(vsatish): This is unnecessary and can be removed for potential + # performance improvements. + fc_bias = self._weights.weights["{}_bias".format(fc_name)] bias_packed = self._pack(tf.shape(convh_im)[1], tf.shape(convh_im)[2], fc_bias, vector=True) - # add bias and apply activation + # Add bias and apply activation. convh = self._leaky_relu(convh + bias_packed, alpha=self._relu_coeff) return convh def _build_im_stream(self, input_node, input_pose_node, input_height, input_width, input_channels, drop_rate, layers, only_stream=False): - self._logger.info('Building Image Stream...') + self._logger.info("Building Image Stream...") if self._input_depth_mode == InputDepthMode.SUB: sub_mean = tf.constant(self._im_depth_sub_mean, dtype=tf.float32) @@ -188,59 +205,59 @@ def _build_im_stream(self, input_node, input_pose_node, input_height, input_widt input_node = norm_sub_im output_node = input_node - prev_layer = "start" # dummy placeholder + prev_layer = "start" # Dummy placeholder. filter_dim = self._train_im_width - last_index = len(layers.keys()) - 1 - for layer_index, (layer_name, layer_config) in enumerate(layers.iteritems()): - layer_type = layer_config['type'] - if layer_type == 'conv': - if prev_layer == 'fc': - raise ValueError('Cannot have conv layer after fc layer!') - output_node, input_height, input_width, input_channels = self._build_conv_layer(output_node, input_height, input_width, input_channels, layer_config['filt_dim'], layer_config['filt_dim'], layer_config['num_filt'], layer_config['pool_stride'], layer_config['pool_stride'], layer_config['pool_size'], layer_name, norm=layer_config['norm'], pad=layer_config['pad']) + last_index = len(layers) - 1 + for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + layer_type = layer_config["type"] + if layer_type == "conv": + if prev_layer == "fc": + raise ValueError("Cannot have conv layer after fc layer!") + output_node, input_height, input_width, input_channels = self._build_conv_layer(output_node, input_height, input_width, input_channels, layer_config["filt_dim"], layer_config["filt_dim"], layer_config["num_filt"], layer_config["pool_stride"], layer_config["pool_stride"], layer_config["pool_size"], layer_name, norm=layer_config["norm"], pad=layer_config["pad"]) prev_layer = layer_type - if layer_config['pad'] == 'SAME': - filter_dim /= layer_config['pool_stride'] + if layer_config["pad"] == "SAME": + filter_dim //= layer_config["pool_stride"] else: - filter_dim = ((filter_dim - layer_config['filt_dim']) / layer_config['pool_stride']) + 1 - elif layer_type == 'fc': + filter_dim = ((filter_dim - layer_config["filt_dim"]) // layer_config["pool_stride"]) + 1 + elif layer_type == "fc": if layer_index == last_index and only_stream: output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name, final_fc_layer=True) else: output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name) prev_layer = layer_type - filter_dim = 1 # because fully-convolutional layers at this point in the network have a filter_dim of 1 - elif layer_type == 'pc': - raise ValueError('Cannot have pose connected layer in image stream!') - elif layer_type == 'fc_merge': - raise ValueError('Cannot have merge layer in image stream!') + filter_dim = 1 # Because fully-convolutional layers at this point in the network have a filter_dim of 1. + elif layer_type == "pc": + raise ValueError("Cannot have pose connected layer in image stream!") + elif layer_type == "fc_merge": + raise ValueError("Cannot have merge layer in image stream!") else: raise ValueError("Unsupported layer type: {}".format(layer_type)) return output_node, -1 def _build_merge_stream(self, input_stream_1, input_stream_2, fan_in_1, fan_in_2, drop_rate, layers): - self._logger.info('Building Merge Stream...') + self._logger.info("Building Merge Stream...") - # first check if first layer is a merge layer - if layers[layers.keys()[0]]['type'] != 'fc_merge': - raise ValueError('First layer in merge stream must be of type fc_merge!') + # First check if first layer is a merge layer. + if layers[list(layers)[0]]["type"] != "fc_merge": + raise ValueError("First layer in merge stream must be of type fc_merge!") - prev_layer = "start" # dummy placeholder - last_index = len(layers.keys()) - 1 - filter_dim = 1 # because fully-convolutional layers at this point in the network have a filter_dim of 1 + prev_layer = "start" # Dummy placeholder. + last_index = len(layers) - 1 + filter_dim = 1 # Because fully-convolutional layers at this point in the network have a filter_dim of 1. fan_in = -1 - for layer_index, (layer_name, layer_config) in enumerate(layers.iteritems()): - layer_type = layer_config['type'] - if layer_type == 'conv': - raise ValueError('Cannot have conv layer in merge stream!') - elif layer_type == 'fc': + for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + layer_type = layer_config["type"] + if layer_type == "conv": + raise ValueError("Cannot have conv layer in merge stream!") + elif layer_type == "fc": if layer_index == last_index: output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name, final_fc_layer=True) else: output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name) prev_layer = layer_type - elif layer_type == 'pc': - raise ValueError('Cannot have pose connected layer in merge stream!') - elif layer_type == 'fc_merge': + elif layer_type == "pc": + raise ValueError("Cannot have pose connected layer in merge stream!") + elif layer_type == "fc_merge": output_node = self._build_fully_conv_merge_layer(input_stream_1, input_stream_2, filter_dim, layer_name) prev_layer = layer_type else: diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index 3ba61bf3..db7dbdb7 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,25 +21,30 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + GQ-CNN network implemented in Tensorflow. Authors: Vishal Satish, Jeff Mahler """ -import json +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from collections import OrderedDict -import os +import json import math -import time import operator +import os import sys +import time import numpy as np import tensorflow as tf import tensorflow.contrib.framework as tcf from autolab_core import Logger -from gqcnn.utils import reduce_shape, read_pose_data, pose_dim, weight_name_to_layer_name, GripperMode, TrainingMode, InputDepthMode +from gqcnn.utils import (reduce_shape, read_pose_data, pose_dim, + weight_name_to_layer_name, GripperMode, + TrainingMode, InputDepthMode) class GQCNNWeights(object): """Helper struct for storing network weights.""" @@ -60,7 +68,7 @@ def __init__(self, gqcnn_config, verbose=True, log_file=None): self._sess = None self._graph = tf.Graph() - # set up logger + # Set up logger. self._logger = Logger.get_logger(self.__class__.__name__, log_file=log_file, silence=(not verbose), global_log_file=verbose) self._weights = GQCNNWeights() @@ -84,132 +92,133 @@ def load(model_dir, verbose=True, log_file=None): :obj:`GQCNNTF` initialized GQ-CNN """ - config_file = os.path.join(model_dir, 'config.json') + config_file = os.path.join(model_dir, "config.json") with open(config_file) as data_file: train_config = json.load(data_file, object_pairs_hook=OrderedDict) - # support for legacy configs + # Support for legacy configs. try: - gqcnn_config = train_config['gqcnn'] + gqcnn_config = train_config["gqcnn"] except: - gqcnn_config = train_config['gqcnn_config'] - - # convert old networks to new flexible arch format - gqcnn_config['debug'] = 0 - gqcnn_config['seed'] = 0 - gqcnn_config['num_angular_bins'] = 0 # legacy networks had no angular support - gqcnn_config['input_depth_mode'] = InputDepthMode.POSE_STREAM # legacy networks only supported depth integration through pose stream - arch_config = gqcnn_config['architecture'] - if 'im_stream' not in arch_config.keys(): + gqcnn_config = train_config["gqcnn_config"] + + # Convert old networks to new flexible arch format. + gqcnn_config["debug"] = 0 + gqcnn_config["seed"] = 0 + gqcnn_config["num_angular_bins"] = 0 # Legacy networks had no angular support. + gqcnn_config["input_depth_mode"] = InputDepthMode.POSE_STREAM # Legacy networks only supported depth integration through pose stream. + arch_config = gqcnn_config["architecture"] + if "im_stream" not in arch_config: new_arch_config = OrderedDict() - new_arch_config['im_stream'] = OrderedDict() - new_arch_config['pose_stream'] = OrderedDict() - new_arch_config['merge_stream'] = OrderedDict() - - layer_name = 'conv1_1' - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'conv' - new_arch_config['im_stream'][layer_name]['pad'] = 'SAME' - if 'padding' in arch_config[layer_name].keys(): - new_arch_config['im_stream'][layer_name]['pad'] = arch_config[layer_name]['padding'] - - layer_name = 'conv1_2' - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'conv' - new_arch_config['im_stream'][layer_name]['pad'] = 'SAME' - if 'padding' in arch_config[layer_name].keys(): - new_arch_config['im_stream'][layer_name]['pad'] = arch_config[layer_name]['padding'] - - layer_name = 'conv2_1' - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'conv' - new_arch_config['im_stream'][layer_name]['pad'] = 'SAME' - if 'padding' in arch_config[layer_name].keys(): - new_arch_config['im_stream'][layer_name]['pad'] = arch_config[layer_name]['padding'] - - layer_name = 'conv2_2' - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'conv' - new_arch_config['im_stream'][layer_name]['pad'] = 'SAME' - if 'padding' in arch_config[layer_name].keys(): - new_arch_config['im_stream'][layer_name]['pad'] = arch_config[layer_name]['padding'] - - layer_name = 'conv3_1' - if layer_name in arch_config.keys(): - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'conv' - new_arch_config['im_stream'][layer_name]['pad'] = 'SAME' - if 'padding' in arch_config[layer_name].keys(): - new_arch_config['im_stream'][layer_name]['pad'] = arch_config[layer_name]['padding'] - - layer_name = 'conv3_2' - if layer_name in arch_config.keys(): - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'conv' - new_arch_config['im_stream'][layer_name]['pad'] = 'SAME' - if 'padding' in arch_config[layer_name].keys(): - new_arch_config['im_stream'][layer_name]['pad'] = arch_config[layer_name]['padding'] - - layer_name = 'fc3' - new_arch_config['im_stream'][layer_name] = arch_config[layer_name] - new_arch_config['im_stream'][layer_name]['type'] = 'fc' + new_arch_config["im_stream"] = OrderedDict() + new_arch_config["pose_stream"] = OrderedDict() + new_arch_config["merge_stream"] = OrderedDict() + + layer_name = "conv1_1" + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name]["pad"] = "SAME" + if "padding" in arch_config[layer_name]: + new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + + layer_name = "conv1_2" + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name]["pad"] = "SAME" + if "padding" in arch_config[layer_name]: + new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + + layer_name = "conv2_1" + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name]["pad"] = "SAME" + if "padding" in arch_config[layer_name]: + new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + + layer_name = "conv2_2" + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name]["pad"] = "SAME" + if "padding" in arch_config[layer_name]: + new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + + layer_name = "conv3_1" + if layer_name in arch_config: + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name]["pad"] = "SAME" + if "padding" in arch_config[layer_name]: + new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + + layer_name = "conv3_2" + if layer_name in arch_config: + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name]["pad"] = "SAME" + if "padding" in arch_config[layer_name]: + new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + + layer_name = "fc3" + new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "fc" - layer_name = 'pc1' - new_arch_config['pose_stream'][layer_name] = arch_config[layer_name] - new_arch_config['pose_stream'][layer_name]['type'] = 'pc' - - layer_name = 'pc2' - if layer_name in arch_config.keys(): - new_arch_config['pose_stream'][layer_name] = arch_config[layer_name] - new_arch_config['pose_stream'][layer_name]['type'] = 'pc' + layer_name = "pc1" + new_arch_config["pose_stream"][layer_name] = arch_config[layer_name] + new_arch_config["pose_stream"][layer_name]["type"] = "pc" + + layer_name = "pc2" + if layer_name in arch_config: + new_arch_config["pose_stream"][layer_name] = arch_config[layer_name] + new_arch_config["pose_stream"][layer_name]["type"] = "pc" - layer_name = 'fc4' - new_arch_config['merge_stream'][layer_name] = arch_config[layer_name] - new_arch_config['merge_stream'][layer_name]['type'] = 'fc_merge' + layer_name = "fc4" + new_arch_config["merge_stream"][layer_name] = arch_config[layer_name] + new_arch_config["merge_stream"][layer_name]["type"] = "fc_merge" - layer_name = 'fc5' - new_arch_config['merge_stream'][layer_name] = arch_config[layer_name] - new_arch_config['merge_stream'][layer_name]['type'] = 'fc' + layer_name = "fc5" + new_arch_config["merge_stream"][layer_name] = arch_config[layer_name] + new_arch_config["merge_stream"][layer_name]["type"] = "fc" - gqcnn_config['architecture'] = new_arch_config + gqcnn_config["architecture"] = new_arch_config - # initialize weights and Tensorflow network + # Initialize weights and Tensorflow network. gqcnn = GQCNNTF(gqcnn_config, verbose=verbose, log_file=log_file) - gqcnn.init_weights_file(os.path.join(model_dir, 'model.ckpt')) + gqcnn.init_weights_file(os.path.join(model_dir, "model.ckpt")) gqcnn.init_mean_and_std(model_dir) - training_mode = train_config['training_mode'] + training_mode = train_config["training_mode"] if training_mode == TrainingMode.CLASSIFICATION: gqcnn.initialize_network(add_softmax=True) elif training_mode == TrainingMode.REGRESSION: gqcnn.initialize_network() else: - raise ValueError('Invalid training mode: {}'.format(training_mode)) + raise ValueError("Invalid training mode: {}".format(training_mode)) return gqcnn def init_mean_and_std(self, model_dir): - """Loads the means and stds of a trained GQ-CNN to use for data normalization during inference. + """Loads the means and stds of a trained GQ-CNN to use for data + normalization during inference. Parameters ---------- model_dir : str - path to trained GQ-CNN model where means and standard deviations are stored + path to trained GQ-CNN model where means and standard deviations + are stored """ - # load in means and stds + # Load in means and stds. if self._input_depth_mode == InputDepthMode.POSE_STREAM: try: - self._im_mean = np.load(os.path.join(model_dir, 'im_mean.npy')) - self._im_std = np.load(os.path.join(model_dir, 'im_std.npy')) + self._im_mean = np.load(os.path.join(model_dir, "im_mean.npy")) + self._im_std = np.load(os.path.join(model_dir, "im_std.npy")) except: - # support for legacy file naming convention - self._im_mean = np.load(os.path.join(model_dir, 'mean.npy')) - self._im_std = np.load(os.path.join(model_dir, 'std.npy')) - self._pose_mean = np.load(os.path.join(model_dir, 'pose_mean.npy')) - self._pose_std = np.load(os.path.join(model_dir, 'pose_std.npy')) - - # fix legacy #TODO: @Jeff, what needs to be fixed here? Or did I add this in? - # read the certain parts of the pose mean/std that we desire + # Support for legacy file naming convention. + self._im_mean = np.load(os.path.join(model_dir, "mean.npy")) + self._im_std = np.load(os.path.join(model_dir, "std.npy")) + self._pose_mean = np.load(os.path.join(model_dir, "pose_mean.npy")) + self._pose_std = np.load(os.path.join(model_dir, "pose_std.npy")) + + # Read the certain parts of the pose mean/std that we desire. if len(self._pose_mean.shape) > 0 and self._pose_mean.shape[0] != self._pose_dim: - # handle multidim storage + # Handle multi-dim storage. if len(self._pose_mean.shape) > 1 and self._pose_mean.shape[1] == self._pose_dim: self._pose_mean = self._pose_mean[0,:] self._pose_std = self._pose_std[0,:] @@ -217,38 +226,39 @@ def init_mean_and_std(self, model_dir): self._pose_mean = read_pose_data(self._pose_mean, self._gripper_mode) self._pose_std = read_pose_data(self._pose_std, self._gripper_mode) elif self._input_depth_mode == InputDepthMode.SUB: - self._im_depth_sub_mean = np.load(os.path.join(model_dir, 'im_depth_sub_mean.npy')) - self._im_depth_sub_std = np.load(os.path.join(model_dir, 'im_depth_sub_std.npy')) + self._im_depth_sub_mean = np.load(os.path.join(model_dir, "im_depth_sub_mean.npy")) + self._im_depth_sub_std = np.load(os.path.join(model_dir, "im_depth_sub_std.npy")) elif self._input_depth_mode == InputDepthMode.IM_ONLY: - self._im_mean = np.load(os.path.join(model_dir, 'im_mean.npy')) - self._im_std = np.load(os.path.join(model_dir, 'im_std.npy')) + self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames."im_mean.npy")) + self._im_std = np.load(os.path.join(model_dir, "im_std.npy")) else: - raise ValueError('Unsupported input depth mode: {}'.format(self._input_depth_mode)) + raise ValueError("Unsupported input depth mode: {}".format(self._input_depth_mode)) def set_base_network(self, model_dir): - """Initialize network weights for the base network. Used during fine-tuning. + """Initialize network weights for the base network. Used during + fine-tuning. Parameters ---------- model_dir : str path to pre-trained GQ-CNN model """ - # check architecture - if 'base_model' not in self._architecture.keys(): - self._logger.warning('Architecuture has no base model. The network has not been modified.') + # Check architecture. + if "base_model" not in self._architecture: + self._logger.warning("Architecuture has no base model. The network has not been modified.") return False - base_model_config = self._architecture['base_model'] - output_layer = base_model_config['output_layer'] + base_model_config = self._architecture["base_model"] + output_layer = base_model_config["output_layer"] - # read model - ckpt_file = os.path.join(model_dir, 'model.ckpt') - config_file = os.path.join(model_dir, 'architecture.json') - base_arch = json.load(open(config_file, 'r'), object_pairs_hook=OrderedDict) + # Read model. + ckpt_file = os.path.join(model_dir, "model.ckpt") + config_file = os.path.join(model_dir, "architecture.json") + base_arch = json.load(open(config_file, "r"), object_pairs_hook=OrderedDict) - # read base layer names + # Read base layer names. self._base_layer_names = [] found_base_layer = False - use_legacy = not ('im_stream' in base_arch.keys()) + use_legacy = not ("im_stream" in base_arch) if use_legacy: layer_iter = iter(base_arch) while not found_base_layer: @@ -273,26 +283,26 @@ def set_base_network(self, model_dir): stop = True with self._graph.as_default(): - # create new tf checkpoint reader + # Create new tf checkpoint reader. reader = tf.train.NewCheckpointReader(ckpt_file) - # create empty weights object + # Create empty weights object. self._weights = GQCNNWeights() - # read/generate weight/bias variable names + # Read/generate weight/bias variable names. ckpt_vars = tcf.list_variables(ckpt_file) full_var_names = [] short_names = [] for variable, shape in ckpt_vars: full_var_names.append(variable) - short_names.append(variable.split('/')[-1]) + short_names.append(variable.split("/")[-1]) - # load variables + # Load variables. for full_var_name, short_name in zip(full_var_names, short_names): - # check valid weights + # Check valid weights. layer_name = weight_name_to_layer_name(short_name) - # add weights + # Add weights. if layer_name in self._base_layer_names: self._weights.weights[short_name] = tf.Variable(reader.get_tensor(full_var_name), name=full_var_name) @@ -305,21 +315,21 @@ def init_weights_file(self, ckpt_file): Tensorflow checkpoint file from which to load model weights """ with self._graph.as_default(): - # create new tf checkpoint reader + # Create new tf checkpoint reader. reader = tf.train.NewCheckpointReader(ckpt_file) - # create empty weight object + # Create empty weight object. self._weights = GQCNNWeights() - # read/generate weight/bias variable names + # Read/generate weight/bias variable names. ckpt_vars = tcf.list_variables(ckpt_file) full_var_names = [] short_names = [] for variable, shape in ckpt_vars: full_var_names.append(variable) - short_names.append(variable.split('/')[-1]) + short_names.append(variable.split("/")[-1]) - # load variables + # Load variables. for full_var_name, short_name in zip(full_var_names, short_names): self._weights.weights[short_name] = tf.Variable(reader.get_tensor(full_var_name), name=full_var_name) @@ -333,59 +343,59 @@ def _parse_config(self, gqcnn_config): """ ##################### PARSING GQCNN CONFIG ##################### - # load tensor params - self._batch_size = gqcnn_config['batch_size'] - self._train_im_height = gqcnn_config['im_height'] - self._train_im_width = gqcnn_config['im_width'] + # Load tensor params. + self._batch_size = gqcnn_config["batch_size"] + self._train_im_height = gqcnn_config["im_height"] + self._train_im_width = gqcnn_config["im_width"] self._im_height = self._train_im_height self._im_width = self._train_im_width - self._num_channels = gqcnn_config['im_channels'] + self._num_channels = gqcnn_config["im_channels"] try: - self._gripper_mode = gqcnn_config['gripper_mode'] + self._gripper_mode = gqcnn_config["gripper_mode"] except: - # legacy support - self._input_data_mode = gqcnn_config['input_data_mode'] - if self._input_data_mode == 'tf_image': + # Legacy support. + self._input_data_mode = gqcnn_config["input_data_mode"] + if self._input_data_mode == "tf_image": self._gripper_mode = GripperMode.LEGACY_PARALLEL_JAW - elif self._input_data_mode == 'tf_image_suction': + elif self._input_data_mode == "tf_image_suction": self._gripper_mode = GripperMode.LEGACY_SUCTION - elif self._input_data_mode == 'suction': + elif self._input_data_mode == "suction": self._gripper_mode = GripperMode.SUCTION - elif self._input_data_mode == 'multi_suction': + elif self._input_data_mode == "multi_suction": self._gripper_mode = GripperMode.MULTI_SUCTION - elif self._input_data_mode == 'parallel_jaw': + elif self._input_data_mode == "parallel_jaw": self._gripper_mode = GripperMode.PARALLEL_JAW else: - raise ValueError('Legacy input data mode: {} not supported!'.format(self._input_data_mode)) - self._logger.warning('Could not read gripper mode. Attempting legacy conversion to: {}'.format(self._gripper_mode)) + raise ValueError("Legacy input data mode: {} not supported!".format(self._input_data_mode)) + self._logger.warning("Could not read gripper mode. Attempting legacy conversion to: {}".format(self._gripper_mode)) - # setup gripper pose dimension depending on gripper mode + # Setup gripper pose dimension depending on gripper mode. self._pose_dim = pose_dim(self._gripper_mode) - # load architecture - self._architecture = gqcnn_config['architecture'] + # Load architecture. + self._architecture = gqcnn_config["architecture"] - # get input depth mode - self._input_depth_mode = InputDepthMode.POSE_STREAM # legacy support - if 'input_depth_mode' in gqcnn_config.keys(): - self._input_depth_mode = gqcnn_config['input_depth_mode'] + # Get input depth mode. + self._input_depth_mode = InputDepthMode.POSE_STREAM # Legacy support. + if "input_depth_mode" in gqcnn_config: + self._input_depth_mode = gqcnn_config["input_depth_mode"] - # load network local response normalization layer constants - self._normalization_radius = gqcnn_config['radius'] - self._normalization_alpha = gqcnn_config['alpha'] - self._normalization_beta = gqcnn_config['beta'] - self._normalization_bias = gqcnn_config['bias'] - - # get ReLU coefficient - self._relu_coeff = 0.0 # legacy support - if 'relu_coeff' in gqcnn_config.keys(): - self._relu_coeff = gqcnn_config['relu_coeff'] - - # debugging - self._debug = gqcnn_config['debug'] - self._rand_seed = gqcnn_config['seed'] - - # initialize means and standard deviations to be 0 and 1, respectively + # Load network local response normalization layer constants. + self._normalization_radius = gqcnn_config["radius"] + self._normalization_alpha = gqcnn_config["alpha"] + self._normalization_beta = gqcnn_config["beta"] + self._normalization_bias = gqcnn_config["bias"] + + # Get ReLU coefficient. + self._relu_coeff = 0.0 # Legacy support. + if "relu_coeff" in gqcnn_config: + self._relu_coeff = gqcnn_config["relu_coeff"] + + # Debugging. + self._debug = gqcnn_config["debug"] + self._rand_seed = gqcnn_config["seed"] + + # Initialize means and standard deviations to be 0 and 1, respectively. if self._input_depth_mode == InputDepthMode.POSE_STREAM: self._im_mean = 0 self._im_std = 1 @@ -398,24 +408,24 @@ def _parse_config(self, gqcnn_config): self._im_mean = 0 self._im_std = 1 - # get number of angular bins - self._angular_bins = 0 # legacy support - if 'angular_bins' in gqcnn_config.keys(): - self._angular_bins = gqcnn_config['angular_bins'] + # Get number of angular bins. + self._angular_bins = 0 # Legacy support. + if "angular_bins" in gqcnn_config: + self._angular_bins = gqcnn_config["angular_bins"] - # get max angle + # Get max angle. self._max_angle = np.pi - if 'max_angle' in gqcnn_config.keys(): - self._max_angle = np.deg2rad(gqcnn_config['max_angle']) + if "max_angle" in gqcnn_config: + self._max_angle = np.deg2rad(gqcnn_config["max_angle"]) - # if using angular bins, make sure output size of final fully connected layer is 2x number of angular bins(because of failure/success probs for each bin) + # If using angular bins, make sure output size of final fully connected layer is 2x number of angular bins (because of failure/success probs for each bin). if self._angular_bins > 0: - assert self._architecture.values()[-1].values()[-1]['out_size'] == 2 * self._angular_bins, 'When predicting angular outputs, output size of final fully connected layer must be 2x number of angular bins' + assert list(list(self._architecture.values())[-1].values())[-1]["out_size"] == 2 * self._angular_bins, "When predicting angular outputs, output size of final fully connected layer must be 2x number of angular bins" - # intermediate network feature handles + # Intermediate network feature handles. self._feature_tensors = {} - # base layer names for fine-tuning + # Base layer names for fine-tuning. self._base_layer_names = [] def initialize_network(self, train_im_node=None, train_pose_node=None, add_softmax=False, add_sigmoid=False): @@ -433,45 +443,45 @@ def initialize_network(self, train_im_node=None, train_pose_node=None, add_softm whether or not to add a sigmoid layer to output of network """ with self._graph.as_default(): - # set tf random seed if debugging + # Set TF random seed if debugging. if self._debug: tf.set_random_seed(self._rand_seed) - # setup input placeholders + # Setup input placeholders. if train_im_node is not None: - # training + # Training. self._input_im_node = tf.placeholder_with_default(train_im_node, (None, self._im_height, self._im_width, self._num_channels)) self._input_pose_node = tf.placeholder_with_default(train_pose_node, (None, self._pose_dim)) else: - # inference only using GQ-CNN instantiated from GQCNNTF.load() + # Inference only using GQ-CNN instantiated from `GQCNNTF.load`. self._input_im_node = tf.placeholder(tf.float32, (self._batch_size, self._im_height, self._im_width, self._num_channels)) self._input_pose_node = tf.placeholder(tf.float32, (self._batch_size, self._pose_dim)) self._input_drop_rate_node = tf.placeholder_with_default(tf.constant(0.0), ()) - # build network + # Build network. self._output_tensor = self._build_network(self._input_im_node, self._input_pose_node, self._input_drop_rate_node) - # add softmax function to output of network(this is optional because 1) we might be doing regression or 2) we are training and Tensorflow has an optimized cross-entropy loss with the softmax already built-in) + # Add softmax function to output of network (this is optional because 1) we might be doing regression or 2) we are training and Tensorflow has an optimized cross-entropy loss with the softmax already built-in). if add_softmax: self.add_softmax_to_output() - # add sigmoid function to output of network(for weighted cross-entropy loss) + # Add sigmoid function to output of network (for weighted cross-entropy loss). if add_sigmoid: self.add_sigmoid_to_output() - # create feed tensors for prediction + # Create feed tensors for prediction. self._input_im_arr = np.zeros((self._batch_size, self._im_height, self._im_width, self._num_channels)) self._input_pose_arr = np.zeros((self._batch_size, self._pose_dim)) def open_session(self): """Open Tensorflow session.""" if self._sess is not None: - self._logger.warning('Found already initialized TF Session...') + self._logger.warning("Found already initialized TF Session...") return self._sess - self._logger.info('Initializing TF Session...') + self._logger.info("Initializing TF Session...") with self._graph.as_default(): init = tf.global_variables_initializer() self.tf_config = tf.ConfigProto() - # allow Tensorflow gpu_growth so Tensorflow does not lock-up all GPU memory + # Allow Tensorflow gpu growth so Tensorflow does not lock-up all GPU memory. self.tf_config.gpu_options.allow_growth = True self._sess = tf.Session(graph=self._graph, config=self.tf_config) self._sess.run(init) @@ -480,15 +490,16 @@ def open_session(self): def close_session(self): """Close Tensorflow session.""" if self._sess is None: - self._logger.warning('No TF Session to close...') + self._logger.warning("No TF Session to close...") return - self._logger.info('Closing TF Session...') + self._logger.info("Closing TF Session...") with self._graph.as_default(): self._sess.close() self._sess = None def __del__(self): - """Destructor that basically just makes sure the Tensorflow session has been closed.""" + """Destructor that basically just makes sure the Tensorflow session + has been closed.""" if self._sess is not None: self.close_session() @@ -558,7 +569,7 @@ def max_angle(self): @property def stride(self): - return reduce(operator.mul, [layer['pool_stride'] for layer in self._architecture['im_stream'].values() if layer['type']=='conv']) + return reduce(operator.mul, [layer["pool_stride"] for layer in self._architecture["im_stream"].values() if layer["type"] == "conv"]) @property def filters(self): @@ -574,12 +585,12 @@ def filters(self): close_sess = True self.open_session() - first_layer_name = self._architecture['im_stream'].keys()[0] + first_layer_name = list(self._architecture["im_stream"])[0] try: - filters = self._sess.run(self._weights.weights['{}_weights'.format(first_layer_name)]) + filters = self._sess.run(self._weights.weights["{}_weights".format(first_layer_name)]) except: - # legacy support - filters = self._sess.run(self._weights.weights['{}W'.format(first_layer_im_stream)]) + # Legacy support. + filters = self._sess.run(self._weights.weights["{}W".format(first_layer_im_stream)]) if close_sess: self.close_session() @@ -616,7 +627,8 @@ def get_im_mean(self): return self.im_mean def set_im_std(self, im_std): - """Update image standard deviation to be used for normalization during inference. + """Update image standard deviation to be used for normalization during + inference. Parameters ---------- @@ -626,7 +638,8 @@ def set_im_std(self, im_std): self._im_std = im_std def get_im_std(self): - """Get the current image standard deviation to be used for normalization during inference. + """Get the current image standard deviation to be used for + normalization during inference. Returns ------- @@ -646,7 +659,8 @@ def set_pose_mean(self, pose_mean): self._pose_mean = pose_mean def get_pose_mean(self): - """Get the current pose mean to be used for normalization during inference. + """Get the current pose mean to be used for normalization during + inference. Returns ------- @@ -656,7 +670,8 @@ def get_pose_mean(self): return self._pose_mean def set_pose_std(self, pose_std): - """Update pose standard deviation to be used for normalization during inference. + """Update pose standard deviation to be used for normalization during + inference. Parameters ---------- @@ -666,7 +681,8 @@ def set_pose_std(self, pose_std): self._pose_std = pose_std def get_pose_std(self): - """Get the current pose standard deviation to be used for normalization during inference. + """Get the current pose standard deviation to be used for normalization + during inference. Returns ------- @@ -676,7 +692,8 @@ def get_pose_std(self): return self._pose_std def set_im_depth_sub_mean(self, im_depth_sub_mean): - """Update mean of subtracted image and gripper depth to be used for normalization during inference. + """Update mean of subtracted image and gripper depth to be used for + normalization during inference. Parameters ---------- @@ -686,7 +703,8 @@ def set_im_depth_sub_mean(self, im_depth_sub_mean): self._im_depth_sub_mean = im_depth_sub_mean def set_im_depth_sub_std(self, im_depth_sub_std): - """Update standard deviation of subtracted image and gripper depth to be used for normalization during inference. + """Update standard deviation of subtracted image and gripper depth to + be used for normalization during inference. Parameters ---------- @@ -697,20 +715,20 @@ def set_im_depth_sub_std(self, im_depth_sub_std): def add_softmax_to_output(self): """Adds softmax to output of network.""" - with tf.name_scope('softmax'): + with tf.name_scope("softmax"): if self._angular_bins > 0: - self._logger.info('Building Pair-wise Softmax Layer...') + self._logger.info("Building Pair-wise Softmax Layer...") binwise_split_output = tf.split(self._output_tensor, self._angular_bins, axis=-1) binwise_split_output_soft = [tf.nn.softmax(s) for s in binwise_split_output] self._output_tensor = tf.concat(binwise_split_output_soft, -1) else: - self._logger.info('Building Softmax Layer...') + self._logger.info("Building Softmax Layer...") self._output_tensor = tf.nn.softmax(self._output_tensor) def add_sigmoid_to_output(self): """Adds sigmoid to output of network.""" - with tf.name_scope('sigmoid'): - self._logger.info('Building Sigmoid Layer...') + with tf.name_scope("sigmoid"): + self._logger.info("Building Sigmoid Layer...") self._output_tensor = tf.nn.sigmoid(self._output_tensor) def update_batch_size(self, batch_size): @@ -733,32 +751,33 @@ def _predict(self, image_arr, pose_arr, verbose=False): pose_arr : :obj:`numpy.ndarray` input gripper poses verbose : bool - whether or not to log progress to stdout, useful to turn off during training + whether or not to log progress to stdout, useful to turn off during + training """ - # get prediction start time + # Get prediction start time. start_time = time.time() if verbose: - self._logger.info('Predicting...') + self._logger.info("Predicting...") - # setup for prediction + # Setup for prediction. num_batches = math.ceil(image_arr.shape[0] / self._batch_size) num_images = image_arr.shape[0] num_poses = pose_arr.shape[0] output_arr = None if num_images != num_poses: - raise ValueError('Must provide same number of images as poses!') + raise ValueError("Must provide same number of images as poses!") - # predict in batches + # Predict in batches. with self._graph.as_default(): if self._sess is None: - raise RuntimeError('No TF Session open. Please call open_session() first.') + raise RuntimeError("No TF Session open. Please call open_session() first.") i = 0 batch_idx = 0 while i < num_images: if verbose: - self._logger.info('Predicting batch {} of {}...'.format(batch_idx, num_batches)) + self._logger.info("Predicting batch {} of {}...".format(batch_idx, num_batches)) batch_idx += 1 dim = min(self._batch_size, num_images - i) cur_ind = i @@ -780,23 +799,23 @@ def _predict(self, image_arr, pose_arr, verbose=False): feed_dict={self._input_im_node: self._input_im_arr, self._input_pose_node: self._input_pose_arr}) - # allocate output tensor + # Allocate output tensor. if output_arr is None: output_arr = np.zeros([num_images] + list(gqcnn_output.shape[1:])) output_arr[cur_ind:end_ind, :] = gqcnn_output[:dim, :] i = end_ind - # get total prediction time + # Get total prediction time. pred_time = time.time() - start_time if verbose: - self._logger.info('Prediction took {} seconds.'.format(pred_time)) + self._logger.info("Prediction took {} seconds.".format(pred_time)) return output_arr def predict(self, image_arr, pose_arr, verbose=False): - """ - Predict the probability of grasp success given a depth image and gripper pose + """Predict the probability of grasp success given a depth image and + gripper pose Parameters ---------- @@ -805,11 +824,12 @@ def predict(self, image_arr, pose_arr, verbose=False): pose_arr : :obj:`numpy ndarray` tensor of gripper poses verbose : bool - whether or not to log progress to stdout, useful to turn off during training + whether or not to log progress to stdout, useful to turn off during + training """ return self._predict(image_arr, pose_arr, verbose=verbose) - def featurize(self, image_arr, pose_arr=None, feature_layer='conv1_1', verbose=False): + def featurize(self, image_arr, pose_arr=None, feature_layer="conv1_1", verbose=False): """Featurize a set of inputs. Parameters @@ -823,32 +843,32 @@ def featurize(self, image_arr, pose_arr=None, feature_layer='conv1_1', verbose=F verbose : bool whether or not to log progress to stdout """ - # get featurization start time + # Get featurization start time. start_time = time.time() if verbose: - self._logger.info('Featurizing...') + self._logger.info("Featurizing...") - if feature_layer not in self._feature_tensors.keys(): - raise ValueError('Feature layer: {} not recognized.'.format(feature_layer)) + if feature_layer not in self._feature_tensors: + raise ValueError("Feature layer: {} not recognized.".format(feature_layer)) - # setup for featurization + # Setup for featurization. num_images = image_arr.shape[0] if pose_arr is not None: num_poses = pose_arr.shape[0] if num_images != num_poses: - raise ValueError('Must provide same number of images as poses!') + raise ValueError("Must provide same number of images as poses!") output_arr = None - # featurize in batches + # Featurize in batches. with self._graph.as_default(): if self._sess is None: - raise RuntimeError('No TF Session open. Please call open_session() first.') + raise RuntimeError("No TF Session open. Please call open_session() first.") i = 0 while i < num_images: if verbose: - self._logger.info('Featurizing {} of {}...'.format(i, num_images)) + self._logger.info("Featurizing {} of {}...".format(i, num_images)) dim = min(self._batch_size, num_images - i) cur_ind = i end_ind = cur_ind + dim @@ -873,47 +893,47 @@ def featurize(self, image_arr, pose_arr=None, feature_layer='conv1_1', verbose=F i = end_ind if verbose: - self._logger.info('Featurization took {} seconds'.format(time.time() - start_time)) + self._logger.info("Featurization took {} seconds".format(time.time() - start_time)) - # truncate extraneous values off of end of output_arr - output_arr = output_arr[:num_images] #TODO: @Jeff, this isn't needed, right? + # Truncate extraneous values off of end of `output_arr`. + output_arr = output_arr[:num_images] # TODO(vsatish): This isn"t needed, right? return output_arr def _leaky_relu(self, x, alpha=.1): return tf.maximum(alpha * x, x) - def _build_conv_layer(self, input_node, input_height, input_width, input_channels, filter_h, filter_w, num_filt, pool_stride_h, pool_stride_w, pool_size, name, norm=False, pad='SAME'): - self._logger.info('Building convolutional layer: {}...'.format(name)) + def _build_conv_layer(self, input_node, input_height, input_width, input_channels, filter_h, filter_w, num_filt, pool_stride_h, pool_stride_w, pool_size, name, norm=False, pad="SAME"): + self._logger.info("Building convolutional layer: {}...".format(name)) with tf.name_scope(name): - # initialize weights - if '{}_weights'.format(name) in self._weights.weights.keys(): - convW = self._weights.weights['{}_weights'.format(name)] - convb = self._weights.weights['{}_bias'.format(name)] - elif '{}W'.format(name) in self._weights.weights.keys(): # legacy support - self._logger.info('Using old format for layer {}.'.format(name)) - convW = self._weights.weights['{}W'.format(name)] - convb = self._weights.weights['{}b'.format(name)] + # Initialize weights. + if "{}_weights".format(name) in self._weights.weights: + convW = self._weights.weights["{}_weights".format(name)] + convb = self._weights.weights["{}_bias".format(name)] + elif "{}W".format(name) in self._weights.weights: # Legacy support. + self._logger.info("Using old format for layer {}.".format(name)) + convW = self._weights.weights["{}W".format(name)] + convb = self._weights.weights["{}b".format(name)] else: - self._logger.info('Reinitializing layer {}.'.format(name)) + self._logger.info("Reinitializing layer {}.".format(name)) convW_shape = [filter_h, filter_w, input_channels, num_filt] fan_in = filter_h * filter_w * input_channels - std = np.sqrt(2.0 / (fan_in)) - convW = tf.Variable(tf.truncated_normal(convW_shape, stddev=std), name='{}_weights'.format(name)) - convb = tf.Variable(tf.truncated_normal([num_filt], stddev=std), name='{}_bias'.format(name)) + std = np.sqrt(2 / fan_in) + convW = tf.Variable(tf.truncated_normal(convW_shape, stddev=std), name="{}_weights".format(name)) + convb = tf.Variable(tf.truncated_normal([num_filt], stddev=std), name="{}_bias".format(name)) - self._weights.weights['{}_weights'.format(name)] = convW - self._weights.weights['{}_bias'.format(name)] = convb + self._weights.weights["{}_weights".format(name)] = convW + self._weights.weights["{}_bias".format(name)] = convb - if pad == 'SAME': - out_height = input_height / pool_stride_h - out_width = input_width / pool_stride_w + if pad == "SAME": + out_height = input_height // pool_stride_h + out_width = input_width // pool_stride_w else: - out_height = math.ceil(float(input_height - filter_h + 1) / pool_stride_h) - out_width = math.ceil(float(input_width - filter_w + 1) / pool_stride_w) + out_height = math.ceil((input_height - filter_h + 1) / pool_stride_h) + out_width = math.ceil((input_width - filter_w + 1) / pool_stride_w) out_channels = num_filt - # build layer + # Build layer. convh = tf.nn.conv2d(input_node, convW, strides=[ 1, 1, 1, 1], padding=pad) + convb convh = self._leaky_relu(convh, alpha=self._relu_coeff) @@ -928,37 +948,37 @@ def _build_conv_layer(self, input_node, input_height, input_width, input_channel ksize=[1, pool_size, pool_size, 1], strides=[1, pool_stride_h, pool_stride_w, 1], - padding='SAME') + padding="SAME") - # add output to feature dict + # Add output to feature dict. self._feature_tensors[name] = pool return pool, out_height, out_width, out_channels def _build_fc_layer(self, input_node, fan_in, out_size, name, input_is_multi, drop_rate, final_fc_layer=False): - self._logger.info('Building fully connected layer: {}...'.format(name)) + self._logger.info("Building fully connected layer: {}...".format(name)) - # initialize weights - if '{}_weights'.format(name) in self._weights.weights.keys(): - fcW = self._weights.weights['{}_weights'.format(name)] - fcb = self._weights.weights['{}_bias'.format(name)] - elif '{}W'.format(name) in self._weights.weights.keys(): # legacy support - self._logger.info('Using old format for layer {}.'.format(name)) - fcW = self._weights.weights['{}W'.format(name)] - fcb = self._weights.weights['{}b'.format(name)] + # Initialize weights. + if "{}_weights".format(name) in self._weights.weights: + fcW = self._weights.weights["{}_weights".format(name)] + fcb = self._weights.weights["{}_bias".format(name)] + elif "{}W".format(name) in self._weights.weights: # Legacy support. + self._logger.info("Using old format for layer {}.".format(name)) + fcW = self._weights.weights["{}W".format(name)] + fcb = self._weights.weights["{}b".format(name)] else: - self._logger.info('Reinitializing layer {}.'.format(name)) - std = np.sqrt(2.0 / (fan_in)) - fcW = tf.Variable(tf.truncated_normal([fan_in, out_size], stddev=std), name='{}_weights'.format(name)) + self._logger.info("Reinitializing layer {}.".format(name)) + std = np.sqrt(2 / fan_in) + fcW = tf.Variable(tf.truncated_normal([fan_in, out_size], stddev=std), name="{}_weights".format(name)) if final_fc_layer: - fcb = tf.Variable(tf.constant(0.0, shape=[out_size]), name='{}_bias'.format(name)) + fcb = tf.Variable(tf.constant(0.0, shape=[out_size]), name="{}_bias".format(name)) else: - fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), name='{}_bias'.format(name)) + fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), name="{}_bias".format(name)) - self._weights.weights['{}_weights'.format(name)] = fcW - self._weights.weights['{}_bias'.format(name)] = fcb + self._weights.weights["{}_weights".format(name)] = fcW + self._weights.weights["{}_bias".format(name)] = fcb - # build layer + # Build layer. if input_is_multi: reduced_dim1 = reduce_shape(input_node.get_shape()) input_node = tf.reshape(input_node, [-1, reduced_dim1]) @@ -969,80 +989,80 @@ def _build_fc_layer(self, input_node, fan_in, out_size, name, input_is_multi, dr fc = tf.nn.dropout(fc, 1 - drop_rate) - # add output to feature dict + # Add output to feature dict. self._feature_tensors[name] = fc return fc, out_size - #TODO: This really doesn't need to it's own layer type...it does the same thing as _build_fc_layer() + # TODO(vsatish): This really doesn"t need to it"s own layer type...it does the same thing as `_build_fc_layer`. def _build_pc_layer(self, input_node, fan_in, out_size, name): - self._logger.info('Building Fully Connected Pose Layer: {}...'.format(name)) + self._logger.info("Building Fully Connected Pose Layer: {}...".format(name)) - # initialize weights - if '{}_weights'.format(name) in self._weights.weights.keys(): - pcW = self._weights.weights['{}_weights'.format(name)] - pcb = self._weights.weights['{}_bias'.format(name)] - elif '{}W'.format(name) in self._weights.weights.keys(): # legacy support - self._logger.info('Using old format for layer {}'.format(name)) - pcW = self._weights.weights['{}W'.format(name)] - pcb = self._weights.weights['{}b'.format(name)] + # Initialize weights. + if "{}_weights".format(name) in self._weights.weights: + pcW = self._weights.weights["{}_weights".format(name)] + pcb = self._weights.weights["{}_bias".format(name)] + elif "{}W".format(name) in self._weights.weights: # Legacy support. + self._logger.info("Using old format for layer {}".format(name)) + pcW = self._weights.weights["{}W".format(name)] + pcb = self._weights.weights["{}b".format(name)] else: - self._logger.info('Reinitializing layer {}'.format(name)) - std = np.sqrt(2.0 / (fan_in)) + self._logger.info("Reinitializing layer {}".format(name)) + std = np.sqrt(2 / fan_in) pcW = tf.Variable(tf.truncated_normal([fan_in, out_size], - stddev=std), name='{}_weights'.format(name)) + stddev=std), name="{}_weights".format(name)) pcb = tf.Variable(tf.truncated_normal([out_size], - stddev=std), name='{}_bias'.format(name)) + stddev=std), name="{}_bias".format(name)) - self._weights.weights['{}_weights'.format(name)] = pcW - self._weights.weights['{}_bias'.format(name)] = pcb + self._weights.weights["{}_weights".format(name)] = pcW + self._weights.weights["{}_bias".format(name)] = pcb - # build layer + # Build layer. pc = self._leaky_relu(tf.matmul(input_node, pcW) + pcb, alpha=self._relu_coeff) - # add output to feature dict + # Add output to feature dict. self._feature_tensors[name] = pc return pc, out_size def _build_fc_merge(self, input_fc_node_1, input_fc_node_2, fan_in_1, fan_in_2, out_size, drop_rate, name): - self._logger.info('Building Merge Layer: {}...'.format(name)) + self._logger.info("Building Merge Layer: {}...".format(name)) - # initialize weights - if '{}_input_1_weights'.format(name) in self._weights.weights.keys(): - input1W = self._weights.weights['{}_input_1_weights'.format(name)] - input2W = self._weights.weights['{}_input_2_weights'.format(name)] - fcb = self._weights.weights['{}_bias'.format(name)] - elif '{}W_im'.format(name) in self._weights.weights.keys(): # legacy support - self._logger.info('Using old format for layer {}.'.format(name)) - input1W = self._weights.weights['{}W_im'.format(name)] - input2W = self._weights.weights['{}W_pose'.format(name)] - fcb = self._weights.weights['{}b'.format(name)] + # Initialize weights. + if "{}_input_1_weights".format(name) in self._weights.weights: + input1W = self._weights.weights["{}_input_1_weights".format(name)] + input2W = self._weights.weights["{}_input_2_weights".format(name)] + fcb = self._weights.weights["{}_bias".format(name)] + elif "{}W_im".format(name) in self._weights.weights: # Legacy support. + self._logger.info("Using old format for layer {}.".format(name)) + input1W = self._weights.weights["{}W_im".format(name)] + input2W = self._weights.weights["{}W_pose".format(name)] + fcb = self._weights.weights["{}b".format(name)] else: - self._logger.info('Reinitializing layer {}.'.format(name)) - std = np.sqrt(2.0 / (fan_in_1 + fan_in_2)) - input1W = tf.Variable(tf.truncated_normal([fan_in_1, out_size], stddev=std), name='{}_input_1_weights'.format(name)) - input2W = tf.Variable(tf.truncated_normal([fan_in_2, out_size], stddev=std), name='{}_input_2_weights'.format(name)) - fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), name='{}_bias'.format(name)) + self._logger.info("Reinitializing layer {}.".format(name)) + std = np.sqrt(2 / (fan_in_1 + fan_in_2)) + input1W = tf.Variable(tf.truncated_normal([fan_in_1, out_size], stddev=std), name="{}_input_1_weights".format(name)) + input2W = tf.Variable(tf.truncated_normal([fan_in_2, out_size], stddev=std), name="{}_input_2_weights".format(name)) + fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), name="{}_bias".format(name)) - self._weights.weights['{}_input_1_weights'.format(name)] = input1W - self._weights.weights['{}_input_2_weights'.format(name)] = input2W - self._weights.weights['{}_bias'.format(name)] = fcb + self._weights.weights["{}_input_1_weights".format(name)] = input1W + self._weights.weights["{}_input_2_weights".format(name)] = input2W + self._weights.weights["{}_bias".format(name)] = fcb - # build layer + # Build layer. fc = self._leaky_relu(tf.matmul(input_fc_node_1, input1W) + tf.matmul(input_fc_node_2, input2W) + fcb, alpha=self._relu_coeff) fc = tf.nn.dropout(fc, 1 - drop_rate) - # add output to feature dict + # Add output to feature dict. self._feature_tensors[name] = fc return fc, out_size def _build_im_stream(self, input_node, input_pose_node, input_height, input_width, input_channels, drop_rate, layers, only_stream=False): - self._logger.info('Building Image Stream...') + self._logger.info("Building Image Stream...") if self._input_depth_mode == InputDepthMode.SUB: sub_mean = tf.constant(self._im_depth_sub_mean, dtype=tf.float32) @@ -1052,51 +1072,51 @@ def _build_im_stream(self, input_node, input_pose_node, input_height, input_widt input_node = norm_sub_im output_node = input_node - prev_layer = "start" # dummy placeholder - last_index = len(layers.keys()) - 1 - for layer_index, (layer_name, layer_config) in enumerate(layers.iteritems()): - layer_type = layer_config['type'] - if layer_type == 'conv': - if prev_layer == 'fc': - raise ValueError('Cannot have conv layer after fc layer!') - output_node, input_height, input_width, input_channels = self._build_conv_layer(output_node, input_height, input_width, input_channels, layer_config['filt_dim'], layer_config['filt_dim'], layer_config['num_filt'], layer_config['pool_stride'], layer_config['pool_stride'], layer_config['pool_size'], layer_name, norm=layer_config['norm'], pad=layer_config['pad']) + prev_layer = "start" # Dummy placeholder. + last_index = len(layers) - 1 + for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + layer_type = layer_config["type"] + if layer_type == "conv": + if prev_layer == "fc": + raise ValueError("Cannot have conv layer after fc layer!") + output_node, input_height, input_width, input_channels = self._build_conv_layer(output_node, input_height, input_width, input_channels, layer_config["filt_dim"], layer_config["filt_dim"], layer_config["num_filt"], layer_config["pool_stride"], layer_config["pool_stride"], layer_config["pool_size"], layer_name, norm=layer_config["norm"], pad=layer_config["pad"]) prev_layer = layer_type - elif layer_type == 'fc': - if layer_config['out_size'] == 0: + elif layer_type == "fc": + if layer_config["out_size"] == 0: continue prev_layer_is_conv = False - if prev_layer == 'conv': + if prev_layer == "conv": prev_layer_is_conv = True fan_in = input_height * input_width * input_channels if layer_index == last_index and only_stream: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config['out_size'], layer_name, prev_layer_is_conv, drop_rate, final_fc_layer=True) + output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, prev_layer_is_conv, drop_rate, final_fc_layer=True) else: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config['out_size'], layer_name, prev_layer_is_conv, drop_rate) + output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, prev_layer_is_conv, drop_rate) prev_layer = layer_type - elif layer_type == 'pc': - raise ValueError('Cannot have pose connected layer in image stream!') - elif layer_type == 'fc_merge': - raise ValueError('Cannot have merge layer in image stream!') + elif layer_type == "pc": + raise ValueError("Cannot have pose connected layer in image stream!") + elif layer_type == "fc_merge": + raise ValueError("Cannot have merge layer in image stream!") else: raise ValueError("Unsupported layer type: {}".format(layer_type)) return output_node, fan_in def _build_pose_stream(self, input_node, fan_in, layers): - self._logger.info('Building Pose Stream...') + self._logger.info("Building Pose Stream...") output_node = input_node - prev_layer = "start" # dummy placeholder - for layer_name, layer_config in layers.iteritems(): - layer_type = layer_config['type'] - if layer_type == 'conv': - raise ValueError('Cannot have conv layer in pose stream') - elif layer_type == 'fc': - raise ValueError('Cannot have fully connected layer in pose stream') - elif layer_type == 'pc': - if layer_config['out_size'] == 0: + prev_layer = "start" # Dummy placeholder. + for layer_name, layer_config in layers.items(): + layer_type = layer_config["type"] + if layer_type == "conv": + raise ValueError("Cannot have conv layer in pose stream") + elif layer_type == "fc": + raise ValueError("Cannot have fully connected layer in pose stream") + elif layer_type == "pc": + if layer_config["out_size"] == 0: continue - output_node, fan_in = self._build_pc_layer(output_node, fan_in, layer_config['out_size'], layer_name) + output_node, fan_in = self._build_pc_layer(output_node, fan_in, layer_config["out_size"], layer_name) prev_layer = layer_type - elif layer_type == 'fc_merge': + elif layer_type == "fc_merge": raise ValueError("Cannot have merge layer in pose stream") else: raise ValueError("Unsupported layer type: {}".format(layer_type)) @@ -1104,33 +1124,35 @@ def _build_pose_stream(self, input_node, fan_in, layers): return output_node, fan_in def _build_merge_stream(self, input_stream_1, input_stream_2, fan_in_1, fan_in_2, drop_rate, layers): - self._logger.info('Building Merge Stream...') + self._logger.info("Building Merge Stream...") - # first check if first layer is a merge layer - if layers[layers.keys()[0]]['type'] != 'fc_merge': - raise ValueError('First layer in merge stream must be a fc_merge layer!') + # First check if first layer is a merge layer. + # TODO(vsatish): Can't we just get the first value because it's + # ordered? + if layers[list(layers)[0]]["type"] != "fc_merge": + raise ValueError("First layer in merge stream must be a fc_merge layer!") prev_layer = "start" - last_index = len(layers.keys()) - 1 + last_index = len(layers) - 1 fan_in = -1 - for layer_index, (layer_name, layer_config) in enumerate(layers.iteritems()): - layer_type = layer_config['type'] - if layer_type == 'conv': - raise ValueError('Cannot have conv layer in merge stream!') - elif layer_type == 'fc': - if layer_config['out_size'] == 0: + for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + layer_type = layer_config["type"] + if layer_type == "conv": + raise ValueError("Cannot have conv layer in merge stream!") + elif layer_type == "fc": + if layer_config["out_size"] == 0: continue if layer_index == last_index: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config['out_size'], layer_name, False, drop_rate, final_fc_layer=True) + output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, False, drop_rate, final_fc_layer=True) else: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config['out_size'], layer_name, False, drop_rate) + output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, False, drop_rate) prev_layer = layer_type - elif layer_type == 'pc': - raise ValueError('Cannot have pose connected layer in merge stream!') - elif layer_type == 'fc_merge': - if layer_config['out_size'] == 0: + elif layer_type == "pc": + raise ValueError("Cannot have pose connected layer in merge stream!") + elif layer_type == "fc_merge": + if layer_config["out_size"] == 0: continue - output_node, fan_in = self._build_fc_merge(input_stream_1, input_stream_2, fan_in_1, fan_in_2, layer_config['out_size'], drop_rate, layer_name) + output_node, fan_in = self._build_fc_merge(input_stream_1, input_stream_2, fan_in_1, fan_in_2, layer_config["out_size"], drop_rate, layer_name) prev_layer = layer_type else: raise ValueError("Unsupported layer type: {}".format(layer_type)) @@ -1153,16 +1175,16 @@ def _build_network(self, input_im_node, input_pose_node, input_drop_rate_node): :obj:`tf.Tensor` tensor output of network """ - self._logger.info('Building Network...') + self._logger.info("Building Network...") if self._input_depth_mode == InputDepthMode.POSE_STREAM: - assert 'pose_stream' in self._architecture.keys() and 'merge_stream' in self._architecture.keys(), 'When using input depth mode "pose_stream", both pose stream and merge stream must be present!' - with tf.name_scope('im_stream'): - output_im_stream, fan_out_im = self._build_im_stream(input_im_node, input_pose_node, self._im_height, self._im_width, self._num_channels, input_drop_rate_node, self._architecture['im_stream']) - with tf.name_scope('pose_stream'): - output_pose_stream, fan_out_pose = self._build_pose_stream(input_pose_node, self._pose_dim, self._architecture['pose_stream']) - with tf.name_scope('merge_stream'): - return self._build_merge_stream(output_im_stream, output_pose_stream, fan_out_im, fan_out_pose, input_drop_rate_node, self._architecture['merge_stream'])[0] + assert "pose_stream" in self._architecture and "merge_stream" in self._architecture, "When using input depth mode \"pose_stream\", both pose stream and merge stream must be present!" + with tf.name_scope("im_stream"): + output_im_stream, fan_out_im = self._build_im_stream(input_im_node, input_pose_node, self._im_height, self._im_width, self._num_channels, input_drop_rate_node, self._architecture["im_stream"]) + with tf.name_scope("pose_stream"): + output_pose_stream, fan_out_pose = self._build_pose_stream(input_pose_node, self._pose_dim, self._architecture["pose_stream"]) + with tf.name_scope("merge_stream"): + return self._build_merge_stream(output_im_stream, output_pose_stream, fan_out_im, fan_out_pose, input_drop_rate_node, self._architecture["merge_stream"])[0] elif self._input_depth_mode == InputDepthMode.SUB or self._input_depth_mode == InputDepthMode.IM_ONLY: - assert not ('pose_stream' in self._architecture.keys() or 'merge_stream' in self._architecture.keys()), 'When using input depth mode "{}", only im stream is allowed!'.format(self._input_depth_mode) - with tf.name_scope('im_stream'): - return self._build_im_stream(input_im_node, input_pose_node, self._im_height, self._im_width, self._num_channels, input_drop_rate_node, self._architecture['im_stream'], only_stream=True)[0] + assert not ("pose_stream" in self._architecture or "merge_stream" in self._architecture), "When using input depth mode \"{}\", only im stream is allowed!".format(self._input_depth_mode) + with tf.name_scope("im_stream"): + return self._build_im_stream(input_im_node, input_pose_node, self._im_height, self._im_width, self._num_channels, input_drop_rate_node, self._architecture["im_stream"], only_stream=True)[0] diff --git a/gqcnn/search/__init__.py b/gqcnn/search/__init__.py index c6aec589..6cdbab6c 100644 --- a/gqcnn/search/__init__.py +++ b/gqcnn/search/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,6 +22,10 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from search import GQCNNSearch +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['GQCNNSearch'] +from .search import GQCNNSearch + +__all__ = ["GQCNNSearch"] diff --git a/gqcnn/search/enums.py b/gqcnn/search/enums.py index fdd9f2c6..17b516c7 100644 --- a/gqcnn/search/enums.py +++ b/gqcnn/search/enums.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,18 +21,19 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Enums for hyper-parameter search. +Enums for hyper-parameter search. Author: Vishal Satish """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -class TrialConstants: - TRIAL_CPU_LOAD = 100 # decrease this to get more aggressize CPU utilization - TRIAL_GPU_LOAD = 20 # decrease this to get more aggressize GPU utilization - TRIAL_GPU_MEM = 2000 # this really depends on model size(TRIAL_GPU_LOAD does too,but it's not a hard limit per se). Ideally we would initialize models one by one and monitor the space left, but because model initialization comes after some metric calculation, we set this to be some upper bound based on the largest model and do batch initalizations from there. +class TrialConstants(object): + TRIAL_CPU_LOAD = 300 # Decrease this to get more aggressize CPU utilization. + TRIAL_GPU_LOAD = 33 # Decrease this to get more aggressize GPU utilization. + TRIAL_GPU_MEM = 2000 # This really depends on model size(TRIAL_GPU_LOAD does too,but it's not a hard limit per se). Ideally we would initialize models one by one and monitor the space left, but because model initialization comes after some metric calculation, we set this to be some upper bound based on the largest model and do batch initalizations from there. -class SearchConstants: +class SearchConstants(object): SEARCH_THREAD_SLEEP = 2 MIN_TIME_BETWEEN_SCHEDULE_ATTEMPTS = 20 diff --git a/gqcnn/search/resource_manager.py b/gqcnn/search/resource_manager.py index 7f4e8618..188998c6 100644 --- a/gqcnn/search/resource_manager.py +++ b/gqcnn/search/resource_manager.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,26 +21,29 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Intelligent resource manager for hyper-parameter search. Queries resources available and appropriately distributes resources over possible trials to run. +Intelligent resource manager for hyper-parameter search. Queries resources +available and appropriately distributes resources over possible trials to run. Author: Vishal Satish """ -import random +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import math -import time -import sys import os +import random +import sys +import time +import GPUtil import numpy as np import psutil -import GPUtil from autolab_core import Logger CPU_LOAD_SAMPLE_INTERVAL = 4.0 -CPU_LOAD_OFFSET = 50 # this is a hack because it seems that psutil is returning a lower load than htop, which could be because htop takes into account queued tasks +CPU_LOAD_OFFSET = 50 # This is a hack because it seems that psutil is returning a lower load than htop, which could be because htop takes into account queued tasks. GPU_STAT_NUM_SAMPLES = 4 GPU_STAT_SAMPLE_INTERVAL = 1.0 @@ -46,13 +52,13 @@ def __init__(self, trial_cpu_load, trial_gpu_load, trial_gpu_mem, monitor_cpu=Tr self._monitor_cpu = monitor_cpu self._monitor_gpu = monitor_gpu - # set up logger + # Set up logger. self._logger = Logger.get_logger(self.__class__.__name__) if not monitor_cpu: - self._logger.warning('Not monitoring cpu resources is not advised.') + self._logger.warning("Not monitoring cpu resources is not advised.") if not monitor_gpu: - self._logger.warning('Not monitoring gpu resources is not advised.') + self._logger.warning("Not monitoring gpu resources is not advised.") self._trial_cpu_load = trial_cpu_load self._trial_gpu_load = trial_gpu_load @@ -60,13 +66,13 @@ def __init__(self, trial_cpu_load, trial_gpu_load, trial_gpu_mem, monitor_cpu=Tr self._cpu_cores = cpu_cores if len(self._cpu_cores) == 0: - self._logger.warning('No CPU cores specified-proceeding to use all available cores.') + self._logger.warning("No CPU cores specified-proceeding to use all available cores.") self._cpu_cores = range(psutil.cpu_count()) self._cpu_count = len(self._cpu_cores) self._gpu_devices = gpu_devices if len(self._gpu_devices) == 0: - self._logger.warning('No GPU devices specified-proceeding to use all available devices.') + self._logger.warning("No GPU devices specified-proceeding to use all available devices.") self._gpu_devices = range(len(GPUtil.getGPUs())) @property @@ -74,7 +80,7 @@ def cpu_cores(self): return self._cpu_cores def _get_cpu_load(self): - self._logger.info('Sampling cpu load...') + self._logger.info("Sampling cpu load...") cpu_core_loads = psutil.cpu_percent(interval=CPU_LOAD_SAMPLE_INTERVAL, percpu=True) total_load = 0 for core in self._cpu_cores: @@ -82,7 +88,7 @@ def _get_cpu_load(self): return total_load + CPU_LOAD_OFFSET def _get_gpu_stats(self): - self._logger.info('Sampling gpu memory and load...') + self._logger.info("Sampling gpu memory and load...") gpu_samples = [] for _ in range(GPU_STAT_NUM_SAMPLES): gpu_samples.append(GPUtil.getGPUs()) @@ -96,7 +102,7 @@ def _get_gpu_stats(self): if gpu.id in self._gpu_devices: sample_loads[gpu.id, i] = gpu.load * 100 sample_mems[gpu.id, i] = gpu.memoryUsed - else: # trick the manager into thinking the GPU is fully utilized so it will never be chosen + else: # Trick the manager into thinking the GPU is fully utilized so it will never be chosen. sample_loads[gpu.id, i] = 100 sample_mems[gpu.id, i] = gpu.memoryTotal total_mems[gpu.id] = gpu.memoryTotal @@ -107,33 +113,33 @@ def _build_gpu_list(self, max_possible_trials_per_device): for device_id, max_trials in enumerate(max_possible_trials_per_device): for _ in range(max_trials): gpus_avail.append(str(device_id)) - random.shuffle(gpus_avail) # this is because we might truncate this list later because of a more severe resource bottleneck, in which case we want to evenly distribute the load + random.shuffle(gpus_avail) # This is because we might truncate this list later because of a more severe resource bottleneck, in which case we want to evenly distribute the load. return gpus_avail def num_trials_to_schedule(self, num_pending_trials): num_trials_to_schedule = num_pending_trials - if self._monitor_cpu: # check cpu bandwith + if self._monitor_cpu: # Check cpu bandwith. cpu_load = min(self._get_cpu_load(), self._cpu_count * 100) max_possible_trials_cpu = int((self._cpu_count * 100 - cpu_load) // self._trial_cpu_load) - self._logger.info('CPU load: {}%, Max possible trials: {}'.format(cpu_load, max_possible_trials_cpu)) + self._logger.info("CPU load: {}%, Max possible trials: {}".format(cpu_load, max_possible_trials_cpu)) num_trials_to_schedule = min(num_trials_to_schedule, max_possible_trials_cpu) - if self._monitor_gpu: # check gpu bandwith + if self._monitor_gpu: # Check gpu bandwith. total_gpu_mems, gpu_loads, gpu_mems = self._get_gpu_stats() max_possible_trials_gpu_load_per_device = [int((100 - gpu_load) // self._trial_gpu_load) for gpu_load in gpu_loads] max_possible_trials_gpu_mem_per_device = [int((total_gpu_mem - gpu_mem) // self._trial_gpu_mem) for total_gpu_mem, gpu_mem in zip(total_gpu_mems, gpu_mems)] max_possible_trials_gpu_per_device = map(lambda (x, y): min(x, y), zip(max_possible_trials_gpu_load_per_device, max_possible_trials_gpu_mem_per_device)) - self._logger.info('GPU loads: {}, GPU mems: {}, Max possible trials: {}'.format('% '.join([str(gpu_load) for gpu_load in gpu_loads]) + '%', 'MiB '.join([str(gpu_mem) for gpu_mem in gpu_mems]) + 'MiB', sum(max_possible_trials_gpu_per_device))) + self._logger.info("GPU loads: {}, GPU mems: {}, Max possible trials: {}".format("% ".join([str(gpu_load) for gpu_load in gpu_loads]) + "%", "MiB ".join([str(gpu_mem) for gpu_mem in gpu_mems]) + "MiB", sum(max_possible_trials_gpu_per_device))) num_trials_to_schedule = min(num_trials_to_schedule, sum(max_possible_trials_gpu_per_device)) - # build the device list for scheduling trials on specific gpus + # Build the device list for scheduling trials on specific gpus. gpus_avail = self._build_gpu_list(max_possible_trials_gpu_per_device) else: - # just distribute load among gpus + # Just distribute load among gpus. num_gpus = self._get_gpu_count() - trials_per_gpu = int(math.ceil(float(num_trials_to_schedule) / num_gpus)) + trials_per_gpu = int(math.ceil(num_trials_to_schedule / num_gpus)) gpus_avail = self._build_gpu_list([trials_per_gpu] * num_gpus) gpus_avail = gpus_avail[:num_trials_to_schedule] - self._logger.info('Max possible trials overall: {}'.format(num_trials_to_schedule)) + self._logger.info("Max possible trials overall: {}".format(num_trials_to_schedule)) return num_trials_to_schedule, gpus_avail diff --git a/gqcnn/search/search.py b/gqcnn/search/search.py index b49b0a48..a6fef34b 100644 --- a/gqcnn/search/search.py +++ b/gqcnn/search/search.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,56 +21,62 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Perform hyper-parameter search over a set of GQ-CNN model/training parameters. Actively monitor system resources and appropriately schedule trials. +Perform hyper-parameter search over a set of GQ-CNN model/training +parameters. Actively monitor system resources and appropriately schedule +trials. Author: Vishal Satish """ -import multiprocessing +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import math -import time +import multiprocessing import operator import os from Queue import Queue import sys +import time -from resource_manager import ResourceManager -from trial import GQCNNTrainingAndAnalysisTrial, GQCNNFineTuningAndAnalysisTrial -from utils import gen_trial_params, gen_timestamp, log_trial_status -from enums import TrialConstants, SearchConstants +from .resource_manager import ResourceManager +from .trial import (GQCNNTrainingAndAnalysisTrial, + GQCNNFineTuningAndAnalysisTrial) +from .utils import gen_trial_params, gen_timestamp, log_trial_status +from .enums import TrialConstants, SearchConstants from autolab_core import Logger -from gqcnn.utils import GQCNNTrainingStatus + +from ..utils import GQCNNTrainingStatus class GQCNNSearch(object): def __init__(self, analysis_config, train_configs, datasets, split_names, base_models=[], output_dir=None, search_name=None, monitor_cpu=True, monitor_gpu=True, cpu_cores=[], gpu_devices=[]): self._analysis_cfg = analysis_config - # create trial output dir if not specified + # Create trial output dir if not specified. if search_name is None: - search_name = 'gqcnn_hyperparam_search_{}'.format(gen_timestamp()) + search_name = "gqcnn_hyperparam_search_{}".format(gen_timestamp()) if output_dir is None: - output_dir = 'models' + output_dir = "models" self._trial_output_dir = os.path.join(output_dir, search_name) if not os.path.exists(self._trial_output_dir): os.makedirs(self._trial_output_dir) - # set up logger - self._logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(self._trial_output_dir, 'search.log'), global_log_file=True) + # Set up logger. + self._logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(self._trial_output_dir, "search.log"), global_log_file=True) - # init resource manager + # Init resource manager. self._resource_manager = ResourceManager(TrialConstants.TRIAL_CPU_LOAD, TrialConstants.TRIAL_GPU_LOAD, TrialConstants.TRIAL_GPU_MEM, monitor_cpu=monitor_cpu, monitor_gpu=monitor_gpu, cpu_cores=cpu_cores, gpu_devices=gpu_devices) - # parse train configs and generate individual trial parameters + # Parse train configs and generate individual trial parameters. if len(base_models) > 0: - assert len(train_configs) == len(datasets) == len(split_names) == len(base_models), 'Must have equal number of training configs, datasets, split_names, and base models!' + assert len(train_configs) == len(datasets) == len(split_names) == len(base_models), "Must have equal number of training configs, datasets, split_names, and base models!" else: - assert len(train_configs) == len(datasets) == len(split_names), 'Must have equal number of training configs, datasets, and split_names!' - self._logger.info('Generating trial parameters...') + assert len(train_configs) == len(datasets) == len(split_names), "Must have equal number of training configs, datasets, and split_names!" + self._logger.info("Generating trial parameters...") trial_params = gen_trial_params(train_configs, datasets, split_names, base_models=base_models) - # create pending trial queue + # Create pending trial queue. self._trials_pending_queue = Queue() if len(base_models) > 0: for trial_name, hyperparam_summary, train_cfg, dataset, base_model, split_name in trial_params: @@ -76,13 +85,13 @@ def __init__(self, analysis_config, train_configs, datasets, split_names, base_m for trial_name, hyperparam_summary, train_cfg, dataset, split_name in trial_params: self._trials_pending_queue.put(GQCNNTrainingAndAnalysisTrial(self._analysis_cfg, train_cfg, dataset, split_name, self._trial_output_dir, trial_name, hyperparam_summary)) - # create containers to hold running, finished, and errored-out trials + # Create containers to hold running, finished, and errored-out trials. self._trials_running = [] self._trials_finished = [] self._trials_errored = [] def search(self): - self._logger.info('Beginning hyper-parameter search...') + self._logger.info("Beginning hyper-parameter search...") done = False waiting_for_trial_init = False delay_resource_check = False @@ -94,40 +103,40 @@ def search(self): num_trials_finished = len(self._trials_finished) num_trials_errored = len(self._trials_errored) - self._logger.info('----------------------------------------------------') - self._logger.info('Num trials pending: {}'.format(num_trials_pending)) - self._logger.info('Num trials running: {}'.format(num_trials_running)) - self._logger.info('Num trials finished: {}'.format(num_trials_finished)) + self._logger.info("----------------------------------------------------") + self._logger.info("Num trials pending: {}".format(num_trials_pending)) + self._logger.info("Num trials running: {}".format(num_trials_running)) + self._logger.info("Num trials finished: {}".format(num_trials_finished)) if num_trials_errored > 0: - self._logger.info('Num trials errored: {}'.format(num_trials_errored)) + self._logger.info("Num trials errored: {}".format(num_trials_errored)) if num_trials_pending > 0 and not waiting_for_trial_init and (time.time() - last_schedule_attempt_time) > SearchConstants.MIN_TIME_BETWEEN_SCHEDULE_ATTEMPTS: - self._logger.info('Attempting to schedule more trials...') + self._logger.info("Attempting to schedule more trials...") num_trials_to_schedule, gpus_avail = self._resource_manager.num_trials_to_schedule(num_trials_pending) - self._logger.info('Scheduling {} trials'.format(num_trials_to_schedule)) + self._logger.info("Scheduling {} trials".format(num_trials_to_schedule)) if num_trials_to_schedule > 0: - # start trials + # Start trials. for _, gpu in zip(range(num_trials_to_schedule), gpus_avail): trial = self._trials_pending_queue.get() trial.begin(gpu_avail=gpu, cpu_cores_avail=self._resource_manager.cpu_cores) self._trials_running.append(trial) - # block scheduling until trials have started training(this is when we know what resources are still available) + # Block scheduling until trials have started training (this is when we know what resources are still available). waiting_for_trial_init = True last_schedule_attempt_time = time.time() - # check if trials have started training + # Check if trials have started training. if waiting_for_trial_init: training_has_started = [trial.training_status == GQCNNTrainingStatus.TRAINING for trial in self._trials_running] if all(training_has_started): waiting_for_trial_init = False - # log trial status + # Log trial status. if len(self._trials_running) > 0: self._logger.info(log_trial_status(self._trials_running)) - # check if any trials have finished running or errored-out + # Check if any trials have finished running or errored-out. finished_trials_to_move = [] errored_trials_to_move = [] for trial in self._trials_running: @@ -142,14 +151,14 @@ def search(self): for trial in errored_trials_to_move: self._trials_running.remove(trial) - # update stopping criteria and sleep + # Update stopping criteria and sleep. done = (num_trials_pending == 0) and (num_trials_running == 0) time.sleep(SearchConstants.SEARCH_THREAD_SLEEP) - self._logger.info('------------------Successful Trials------------------') + self._logger.info("------------------Successful Trials------------------") self._logger.info(log_trial_status(self._trials_finished)) if len(self._trials_errored) > 0: - self._logger.info('--------------------Failed Trials--------------------') + self._logger.info("--------------------Failed Trials--------------------") self._logger.info(log_trial_status(self._trials_errored)) - self._logger.info('Hyper-parameter search finished in {} seconds.'.format(time.time() - search_start_time)) + self._logger.info("Hyper-parameter search finished in {} seconds.".format(time.time() - search_start_time)) diff --git a/gqcnn/search/trial.py b/gqcnn/search/trial.py index 7ccdd716..64b96f94 100644 --- a/gqcnn/search/trial.py +++ b/gqcnn/search/trial.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,35 +21,35 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Trials for hyper-parameter search. +Trials for hyper-parameter search. Author: Vishal Satish """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +from abc import abstractmethod, ABCMeta +import json import multiprocessing -import sys import os -import json -from abc import abstractmethod, ABCMeta +import sys +from future.utils import with_metaclass import numpy as np -from gqcnn.model import get_gqcnn_model -from gqcnn.training import get_gqcnn_trainer -from gqcnn.utils import GQCNNTrainingStatus -from gqcnn.analysis import GQCNNAnalyzer -from gqcnn.utils import GeneralConstants +from ..model import get_gqcnn_model +from ..training import get_gqcnn_trainer +from ..utils import GeneralConstants, GQCNNTrainingStatus +from ..analysis import GQCNNAnalyzer class TrialStatus: - PENDING = 'pending' - RUNNING = 'running' - FINISHED = 'finished' - EXCEPTION = 'exception' - -class GQCNNTrialWithAnalysis(object): - __metaclass__ = ABCMeta + PENDING = "pending" + RUNNING = "running" + FINISHED = "finished" + EXCEPTION = "exception" +class GQCNNTrialWithAnalysis(with_metaclass(ABCMeta, object)): def __init__(self, analysis_cfg, train_cfg, dataset_dir, split_name, output_dir, model_name, hyperparam_summary): self._analysis_cfg = analysis_cfg self._train_cfg = train_cfg @@ -56,8 +59,8 @@ def __init__(self, analysis_cfg, train_cfg, dataset_dir, split_name, output_dir, self._model_name = model_name self._hyperparam_summary = hyperparam_summary self._manager = multiprocessing.Manager() - self._train_progress_dict = self._build_train_progress_dict() # to communicate with training - self._trial_progress_dict = self._build_trial_progress_dict() # to communicate with trial + self._train_progress_dict = self._build_train_progress_dict() # To communicate with training. + self._trial_progress_dict = self._build_trial_progress_dict() # To communicate with trial. self._process = None def _build_train_progress_dict(self): @@ -72,13 +75,13 @@ def _build_trial_progress_dict(self): def _run(self, trainer): pass - def _run_trial(self, analysis_config, train_config, dataset_dir, split_name, output_dir, model_name, train_progress_dict, trial_progress_dict, hyperparam_summary, gpu_avail="", cpu_cores_avail=[], backend='tf'): - trial_progress_dict['status'] = TrialStatus.RUNNING + def _run_trial(self, analysis_config, train_config, dataset_dir, split_name, output_dir, model_name, train_progress_dict, trial_progress_dict, hyperparam_summary, gpu_avail="", cpu_cores_avail=[], backend="tf"): + trial_progress_dict["status"] = TrialStatus.RUNNING try: os.system("taskset -pc {} {}".format(",".join(str(i) for i in cpu_cores_avail), os.getpid())) os.environ["CUDA_VISIBLE_DEVICES"] = gpu_avail - gqcnn = get_gqcnn_model(backend, verbose=False)(train_config['gqcnn'], verbose=False) + gqcnn = get_gqcnn_model(backend, verbose=False)(train_config["gqcnn"], verbose=False) trainer = get_gqcnn_trainer(backend)(gqcnn, dataset_dir, split_name, @@ -89,45 +92,45 @@ def _run_trial(self, analysis_config, train_config, dataset_dir, split_name, out verbose=False) self._run(trainer) - with open(os.path.join(output_dir, model_name, 'hyperparam_summary.json'), 'wb') as fhandle: + with open(os.path.join(output_dir, model_name, "hyperparam_summary.json"), "wb") as fhandle: json.dump(hyperparam_summary, fhandle, indent=GeneralConstants.JSON_INDENT) - train_progress_dict['training_status'] = 'analyzing' + train_progress_dict["training_status"] = "analyzing" analyzer = GQCNNAnalyzer(analysis_config, verbose=False) _, _, init_train_error, final_train_error, init_train_loss, final_train_loss, init_val_error, final_val_error, norm_final_val_error = analyzer.analyze(os.path.join(output_dir, model_name), output_dir) analysis_dict = {} - analysis_dict['init_train_error'] = init_train_error - analysis_dict['final_train_error'] = final_train_error - analysis_dict['init_train_loss'] = init_train_loss - analysis_dict['final_train_loss'] = final_train_loss - analysis_dict['init_val_error'] = init_val_error - analysis_dict['final_val_error'] = final_val_error - analysis_dict['norm_final_val_error'] = norm_final_val_error - train_progress_dict['analysis'] = analysis_dict - - train_progress_dict['training_status'] = 'finished' - trial_progress_dict['status'] = TrialStatus.FINISHED + analysis_dict["init_train_error"] = init_train_error + analysis_dict["final_train_error"] = final_train_error + analysis_dict["init_train_loss"] = init_train_loss + analysis_dict["final_train_loss"] = final_train_loss + analysis_dict["init_val_error"] = init_val_error + analysis_dict["final_val_error"] = final_val_error + analysis_dict["norm_final_val_error"] = norm_final_val_error + train_progress_dict["analysis"] = analysis_dict + + train_progress_dict["training_status"] = "finished" + trial_progress_dict["status"] = TrialStatus.FINISHED sys.exit(0) except Exception as e: - trial_progress_dict['status'] = TrialStatus.EXCEPTION - trial_progress_dict['error_msg'] = str(e) + trial_progress_dict["status"] = TrialStatus.EXCEPTION + trial_progress_dict["error_msg"] = str(e) sys.exit(0) @property def finished(self): - return self._trial_progress_dict['status'] == TrialStatus.FINISHED + return self._trial_progress_dict["status"] == TrialStatus.FINISHED @property def errored_out(self): - return self._trial_progress_dict['status'] == TrialStatus.EXCEPTION + return self._trial_progress_dict["status"] == TrialStatus.EXCEPTION @property def error_msg(self): - return self._trial_progress_dict['error_msg'] + return self._trial_progress_dict["error_msg"] @property def training_status(self): - return self._train_progress_dict['training_status'] + return self._train_progress_dict["training_status"] def begin(self, gpu_avail="", cpu_cores_avail=[]): self._status = TrialStatus.RUNNING @@ -135,13 +138,13 @@ def begin(self, gpu_avail="", cpu_cores_avail=[]): self._process.start() def __str__(self): - trial_str = 'Trial: {}, Training Stage: {}'.format(self._model_name, self.training_status) - if self.training_status == GQCNNTrainingStatus.TRAINING and not np.isnan(self._train_progress_dict['epoch']): - trial_str += ', Epoch: {}/{}'.format(self._train_progress_dict['epoch'], self._train_cfg['num_epochs']) + trial_str = "Trial: {}, Training Stage: {}".format(self._model_name, self.training_status) + if self.training_status == GQCNNTrainingStatus.TRAINING and not np.isnan(self._train_progress_dict["epoch"]): + trial_str += ", Epoch: {}/{}".format(self._train_progress_dict["epoch"], self._train_cfg["num_epochs"]) if self.errored_out: - trial_str += ', Error message: {}'.format(self.error_msg) - if self.training_status == 'finished': - trial_str += ', Initial train error: {}, Final train error: {}, Initial train loss: {}, Final train loss: {}, Initial val error: {}, Final val error: {}, Norm final val error: {}'.format(self._train_progress_dict['analysis']['init_train_error'], self._train_progress_dict['analysis']['final_train_error'], self._train_progress_dict['analysis']['init_train_loss'], self._train_progress_dict['analysis']['final_train_loss'], self._train_progress_dict['analysis']['init_val_error'], self._train_progress_dict['analysis']['final_val_error'], self._train_progress_dict['analysis']['norm_final_val_error']) + trial_str += ", Error message: {}".format(self.error_msg) + if self.training_status == "finished": + trial_str += ", Initial train error: {}, Final train error: {}, Initial train loss: {}, Final train loss: {}, Initial val error: {}, Final val error: {}, Norm final val error: {}".format(self._train_progress_dict["analysis"]["init_train_error"], self._train_progress_dict["analysis"]["final_train_error"], self._train_progress_dict["analysis"]["init_train_loss"], self._train_progress_dict["analysis"]["final_train_loss"], self._train_progress_dict["analysis"]["init_val_error"], self._train_progress_dict["analysis"]["final_val_error"], self._train_progress_dict["analysis"]["norm_final_val_error"]) return trial_str class GQCNNTrainingAndAnalysisTrial(GQCNNTrialWithAnalysis): diff --git a/gqcnn/search/utils.py b/gqcnn/search/utils.py index 13c31416..aa54a27d 100644 --- a/gqcnn/search/utils.py +++ b/gqcnn/search/utils.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,29 +21,31 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Utility functions for hyper-parameter search. +Utility functions for hyper-parameter search. Author: Vishal Satish """ -import copy +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + from collections import OrderedDict, defaultdict +import copy from datetime import datetime import itertools import numpy as np -from gqcnn.utils import GQCNNTrainingStatus +from ..utils import GQCNNTrainingStatus def get_fields_to_search_over(train_config, prev_keys=[]): fields = [] anchored_fields = defaultdict(list) - for key in train_config.keys(): + for key in train_config: if isinstance(train_config[key], list): prev_keys_copy = copy.deepcopy(prev_keys) prev_keys_copy.append(key) - if isinstance(train_config[key][0], str) and train_config[key][0].startswith('anchor_'): + if isinstance(train_config[key][0], str) and train_config[key][0].startswith("anchor_"): anchored_fields[train_config[key][0]].append(prev_keys_copy) train_config[key] = train_config[key][1:] else: @@ -54,7 +59,7 @@ def get_fields_to_search_over(train_config, prev_keys=[]): return fields, anchored_fields def update_dict(dict1, dict2): - for key, val in dict2.iteritems(): + for key, val in dict2.items(): if key in dict1: dict1[key].extend(val) else: @@ -75,39 +80,39 @@ def set_nested_key(cfg, key, val): def gen_config_summary_dict(hyperparam_combination): summary_dict = {} for key, val in hyperparam_combination: - summary_dict['/'.join(key)] = val + summary_dict["/".join(key)] = val return summary_dict def parse_master_train_config(train_config): configs = [] hyperparam_search_fields, hyperparam_anchored_search_fields = get_fields_to_search_over(train_config) - # ensure a one-to-one mapping between hyperparameters of fields with matching anchor tags - for anchor_tag, fields in hyperparam_anchored_search_fields.iteritems(): + # Ensure a one-to-one mapping between hyperparameters of fields with matching anchor tags. + for anchor_tag, fields in hyperparam_anchored_search_fields.items(): num_params = [] for field in fields: num_params.append(len(get_nested_key(train_config, field))) - assert max(num_params) == min(num_params), 'All fields in anchor tag "{}" do not have the same # of parameters to search over!'.format(anchor_tag) + assert max(num_params) == min(num_params), "All fields in anchor tag "{}" do not have the same # of parameters to search over!".format(anchor_tag) - # if there is nothing to search over just return the given config + # If there is nothing to search over just return the given config. if len(hyperparam_search_fields) == 0 and len(hyperparam_anchored_search_fields) == 0: - return [('', train_config)] + return [("", train_config)] - # generate a list of all the possible hyper-parameters to search over + # Generate a list of all the possible hyper-parameters to search over. hyperparam_search_params = [] - for search_field in hyperparam_search_fields: # normal fields + for search_field in hyperparam_search_fields: # Normal fields. search_field_params = [] for val in get_nested_key(train_config, search_field): search_field_params.append((search_field, val)) hyperparam_search_params.append(search_field_params) - for anchored_fields in hyperparam_anchored_search_fields.values(): # anchored fields + for anchored_fields in hyperparam_anchored_search_fields.values(): # Anchored fields. combinations = [[] for _ in range(len(get_nested_key(train_config, anchored_fields[0])))] for field in anchored_fields: for idx, val in enumerate(get_nested_key(train_config, field)): combinations[idx].append((field, val)) hyperparam_search_params.append(combinations) - # get all permutations of the possible hyper-parameters + # Get all permutations of the possible hyper-parameters. hyperparam_combinations = list(itertools.product(*hyperparam_search_params)) def flatten_combo(combo): flattened = [] @@ -120,7 +125,7 @@ def flatten_combo(combo): return flattened hyperparam_combinations = [flatten_combo(combo) for combo in hyperparam_combinations] - # generate possible configs to search over + # Generate possible configs to search over. for combo in hyperparam_combinations: config = copy.deepcopy(train_config) for field, val in combo: @@ -129,14 +134,14 @@ def flatten_combo(combo): return configs def gen_timestamp(): - return str(datetime.now()).split('.')[0].replace(' ', '_') + return str(datetime.now()).split(".")[0].replace(" ", "_") def gen_trial_params_train(master_train_configs, datasets, split_names): trial_params = [] for master_train_config, dataset, split_name in zip(master_train_configs, datasets, split_names): train_configs = parse_master_train_config(master_train_config) for i, (hyperparam_summary_dict, train_config) in enumerate(train_configs): - trial_name = '{}_{}_trial_{}_{}'.format(dataset.split('/')[-3], split_name, i, gen_timestamp()) + trial_name = "{}_{}_trial_{}_{}".format(dataset.split("/")[-3], split_name, i, gen_timestamp()) trial_params.append((trial_name, hyperparam_summary_dict, train_config, dataset, split_name)) return trial_params @@ -145,7 +150,7 @@ def gen_trial_params_finetune(master_train_configs, datasets, base_models, split for master_train_config, dataset, base_model, split_name in zip(master_train_configs, datasets, base_models, split_names): train_configs = parse_master_train_config(master_train_config) for i, (hyperparam_summary_dict, train_config) in enumerate(train_configs): - trial_name = '{}_{}_trial_{}_{}'.format(dataset.split('/')[-3], split_name, i, gen_timestamp()) + trial_name = "{}_{}_trial_{}_{}".format(dataset.split("/")[-3], split_name, i, gen_timestamp()) trial_params.append((trial_name, hyperparam_summary_dict, train_config, dataset, base_model, split_name)) return trial_params @@ -157,9 +162,8 @@ def gen_trial_params(master_train_configs, datasets, split_names, base_models=[] return gen_trial_params_train(master_train_configs, datasets, split_names) def log_trial_status(trials): - status_str = '--------------------TRIAL STATUS--------------------' + status_str = "--------------------TRIAL STATUS--------------------" for trial in trials: - status_str += '\n' - status_str += '[{}]'.format(str(trial)) + status_str += "\n" + status_str += "[{}]".format(str(trial)) return status_str - diff --git a/gqcnn/training/__init__.py b/gqcnn/training/__init__.py index f08cf163..0010fafd 100644 --- a/gqcnn/training/__init__.py +++ b/gqcnn/training/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,18 +21,24 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Factory functions to obtain GQCNNTrainer class based on chosen deep learning backend. -Currently only Tensorflow is supported. +Factory functions to obtain GQCNNTrainer class based on chosen deep learning +backend. Currently only Tensorflow is supported. Author: Vishal Satish """ -from tf import * +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +from .tf import * -def get_gqcnn_trainer(backend='tf'): +def get_gqcnn_trainer(backend="tf"): """ - Get the GQ-CNN Trainer for the provided backend. Currently only TensorFlow is supported. + Get the GQ-CNN Trainer for the provided backend. + + Note + ---- + Currently only TensorFlow is supported. Parameters ---------- @@ -41,9 +50,8 @@ def get_gqcnn_trainer(backend='tf'): :obj:`gqcnn.training.tf.GQCNNTrainerTF` GQ-CNN Trainer with TensorFlow backend """ - - # return desired GQCNNTrainer instance based on backend - if backend == 'tf': + # Return desired `GQCNNTrainer` instance based on backend. + if backend == "tf": return GQCNNTrainerTF else: - raise ValueError('Invalid backend: {}'.format(backend)) + raise ValueError("Invalid backend: {}".format(backend)) diff --git a/gqcnn/training/tf/__init__.py b/gqcnn/training/tf/__init__.py index 44490093..4fd1003d 100644 --- a/gqcnn/training/tf/__init__.py +++ b/gqcnn/training/tf/__init__.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,6 +22,10 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from trainer_tf import GQCNNTrainerTF +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['GQCNNTrainerTF'] +from .trainer_tf import GQCNNTrainerTF + +__all__ = ["GQCNNTrainerTF"] diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index e58b6a58..8dbe530f 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,16 +21,20 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Trains a GQCNN network using Tensorflow backend. + +Trains a GQ-CNN network using Tensorflow backend. Author: Vishal Satish and Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import collections import copy +import pickle as pkl import json -import cPickle as pkl +import multiprocessing as mp import os import random import shutil @@ -36,7 +43,6 @@ import sys import threading import time -import multiprocessing as mp import Queue import cv2 @@ -45,14 +51,18 @@ import scipy.stats as ss import tensorflow as tf -from autolab_core import BinaryClassificationResult, RegressionResult, TensorDataset, YamlConfig, Logger +from autolab_core import (BinaryClassificationResult, RegressionResult, + TensorDataset, YamlConfig, Logger) from autolab_core.constants import * import autolab_core.utils as utils -from gqcnn.utils import ImageMode, TrainingMode, GripperMode, InputDepthMode, GeneralConstants, TrainStatsLogger, pose_dim, read_pose_data, weight_name_to_layer_name, GQCNNTrainingStatus +from ...utils import (ImageMode, TrainingMode, GripperMode, + InputDepthMode, GeneralConstants, TrainStatsLogger, + pose_dim, read_pose_data, weight_name_to_layer_name, + GQCNNTrainingStatus, GQCNNFilenames) class GQCNNTrainerTF(object): - """ Trains a GQ-CNN with Tensorflow backend. """ + """Trains a GQ-CNN with Tensorflow backend.""" def __init__(self, gqcnn, dataset_dir, @@ -68,7 +78,7 @@ def __init__(self, gqcnn, gqcnn : :obj:`GQCNN` grasp quality neural network to optimize dataset_dir : str - path to the training / validation dataset + path to the training/validation dataset split_name : str name of the split to train on output_dir : str @@ -88,51 +98,51 @@ def __init__(self, gqcnn, self.progress_dict = progress_dict self.finetuning = False - # create a directory for the model + # Create a directory for the model. if self.model_name is None: model_id = utils.gen_experiment_id() - self.model_name = 'model_%s' %(model_id) + self.model_name = "model_{}".format(model_id) self.model_dir = os.path.join(self.output_dir, self.model_name) if not os.path.exists(self.model_dir): os.mkdir(self.model_dir) - # set up logger - self.logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(self.model_dir, 'training.log'), silence=(not verbose), global_log_file=verbose) + # Set up logger. + self.logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(self.model_dir, "training.log"), silence=(not verbose), global_log_file=verbose) - # check default split + # Check default split. if split_name is None: - self.logger.warning('Using default image-wise split.') - self.split_name = 'image_wise' + self.logger.warning("Using default image-wise split.") + self.split_name = "image_wise" - # update cfg for saving - self.cfg['dataset_dir'] = self.dataset_dir - self.cfg['split_name'] = self.split_name + # Update cfg for saving. + self.cfg["dataset_dir"] = self.dataset_dir + self.cfg["split_name"] = self.split_name def _create_loss(self): - """ Creates a loss based on config file + """Creates a loss based on config file. Returns ------- :obj:`tensorflow Tensor` loss """ - if self.cfg['loss'] == 'l2': + if self.cfg["loss"] == "l2": return (1.0 / self.train_batch_size) * tf.nn.l2_loss(tf.subtract(tf.nn.sigmoid(self.train_net_output), self.train_labels_node)) - elif self.cfg['loss'] == 'sparse': + elif self.cfg["loss"] == "sparse": if self._angular_bins > 0: log = tf.reshape(tf.dynamic_partition(self.train_net_output, self.train_pred_mask_node, 2)[1], (-1, 2)) return tf.reduce_mean(tf.nn.sparse_softmax_cross_entropy_with_logits(_sentinel=None, labels=self.train_labels_node, logits=log)) else: return tf.reduce_mean(tf.nn.sparse_softmax_cross_entropy_with_logits(_sentinel=None, labels=self.train_labels_node, logits=self.train_net_output, name=None)) - elif self.cfg['loss'] == 'weighted_cross_entropy': + elif self.cfg["loss"] == "weighted_cross_entropy": return tf.reduce_mean(tf.nn.weighted_cross_entropy_with_logits(targets=tf.reshape(self.train_labels_node, [-1,1]), logits=self.train_net_output, pos_weight=self.pos_weight, name=None)) def _create_optimizer(self, loss, batch, var_list, learning_rate): - """ Create optimizer based on config file + """Create optimizer based on config file. Parameters ---------- @@ -150,61 +160,64 @@ def _create_optimizer(self, loss, batch, var_list, learning_rate): :obj:`tf.train.Optimizer` optimizer """ - # instantiate optimizer - if self.cfg['optimizer'] == 'momentum': + # Instantiate optimizer. + if self.cfg["optimizer"] == "momentum": optimizer = tf.train.MomentumOptimizer(learning_rate, self.momentum_rate) - elif self.cfg['optimizer'] == 'adam': + elif self.cfg["optimizer"] == "adam": optimizer = tf.train.AdamOptimizer(learning_rate) - elif self.cfg['optimizer'] == 'rmsprop': + elif self.cfg["optimizer"] == "rmsprop": optimizer = tf.train.RMSPropOptimizer(learning_rate) else: - raise ValueError('Optimizer %s not supported' %(self.cfg['optimizer'])) + raise ValueError("Optimizer \"{}\" not supported".format(self.cfg["optimizer"])) - # compute gradients + # Compute gradients. gradients, variables = zip(*optimizer.compute_gradients(loss, var_list=var_list)) - # clip gradients to prevent exploding gradient problem + + # Clip gradients to prevent exploding gradient problem. + gradients, global_grad_norm = tf.clip_by_global_norm(gradients, self.max_global_grad_norm) - # generate op to apply gradients + + # Generate op to apply gradients. apply_grads = optimizer.apply_gradients(zip(gradients, variables), global_step=batch) return apply_grads, global_grad_norm def _launch_tensorboard(self): - """ Launches Tensorboard to visualize training """ - FNULL = open(os.devnull, 'w') + """Launches Tensorboard to visualize training.""" + FNULL = open(os.devnull, "w") self.logger.info( "Launching Tensorboard, Please navigate to localhost:{} in your favorite web browser to view summaries".format(self._tensorboard_port)) - self._tensorboard_proc = subprocess.Popen(['tensorboard', '--port', str(self._tensorboard_port),'--logdir', self.summary_dir], stdout=FNULL) + self._tensorboard_proc = subprocess.Popen(["tensorboard", "--port", str(self._tensorboard_port),"--logdir", self.summary_dir], stdout=FNULL) def _close_tensorboard(self): """Closes Tensorboard process.""" - self.logger.info('Closing Tensorboard...') + self.logger.info("Closing Tensorboard...") self._tensorboard_proc.terminate() def train(self): - """ Perform optimization. """ + """Perform optimization.""" with self.gqcnn.tf_graph.as_default(): self._train() def _train(self): - """ Perform optimization. """ + """Perform optimization.""" start_time = time.time() - # run setup + # Run setup. if self.progress_dict is not None: - self.progress_dict['training_status'] = GQCNNTrainingStatus.SETTING_UP + self.progress_dict["training_status"] = GQCNNTrainingStatus.SETTING_UP self._setup() - # build network + # Build network. self.gqcnn.initialize_network(self.input_im_node, self.input_pose_node) - # optimize weights + # Optimize weights. if self.progress_dict is not None: - self.progress_dict['training_status'] = GQCNNTrainingStatus.TRAINING + self.progress_dict["training_status"] = GQCNNTrainingStatus.TRAINING self._optimize_weights() def finetune(self, base_model_dir): - """ Perform fine-tuning. + """Perform fine-tuning. Parameters ---------- @@ -215,100 +228,100 @@ def finetune(self, base_model_dir): self._finetune(base_model_dir) def _finetune(self, base_model_dir): - """ Perform fine-tuning. + """Perform fine-tuning. Parameters ---------- base_model_dir : str path to the pre-trained base model to use """ - # set flag and base model for fine-tuning + # Set flag and base model for fine-tuning. self.finetuning = True self.base_model_dir = base_model_dir - # run setup + # Run setup. self._setup() - # build network + # Build network. self.gqcnn.set_base_network(base_model_dir) self.gqcnn.initialize_network(self.input_im_node, self.input_pose_node) - # optimize weights + # Optimize weights. if self.progress_dict is not None: - self.progress_dict['training_status'] = GQCNNTrainingStatus.TRAINING + self.progress_dict["training_status"] = GQCNNTrainingStatus.TRAINING self._optimize_weights(finetune=True) def _optimize_weights(self, finetune=False): - """ Optimize the network weights. """ + """Optimize the network weights.""" start_time = time.time() - # setup output + # Setup output. self.train_net_output = self.gqcnn.output if self.training_mode == TrainingMode.CLASSIFICATION: - if self.cfg['loss'] == 'weighted_cross_entropy': + if self.cfg["loss"] == "weighted_cross_entropy": self.gqcnn.add_sigmoid_to_output() else: self.gqcnn.add_softmax_to_output() elif self.training_mode == TrainingMode.REGRESSION: self.gqcnn.add_sigmoid_to_output() else: - raise ValueError('Training mode: {} not supported !'.format(self.training_mode)) + raise ValueError("Training mode: {} not supported !".format(self.training_mode)) train_predictions = self.gqcnn.output drop_rate_in = self.gqcnn.input_drop_rate_node self.weights = self.gqcnn.weights - # once weights have been initialized create tf Saver for weights + # Once weights have been initialized, create TF saver for weights. self.saver = tf.train.Saver() - # form loss - with tf.name_scope('loss'): - # part 1: error + # Form loss. + with tf.name_scope("loss"): + # Part 1: error. loss = self._create_loss() unregularized_loss = loss - # part 2: regularization - layer_weights = self.weights.values() - with tf.name_scope('regularization'): + # Part 2: regularization. + layer_weights = list(self.weights.values()) + with tf.name_scope("regularization"): regularizers = tf.nn.l2_loss(layer_weights[0]) for w in layer_weights[1:]: regularizers = regularizers + tf.nn.l2_loss(w) loss += self.train_l2_regularizer * regularizers - # setup learning rate + # Setup learning rate. batch = tf.Variable(0) learning_rate = tf.train.exponential_decay( - self.base_lr, # base learning rate. - batch * self.train_batch_size, # current index into the dataset. - self.decay_step, # decay step. + self.base_lr, # Base learning rate. + batch * self.train_batch_size, # Current index into the dataset. + self.decay_step, # Decay step. self.decay_rate, # decay rate. staircase=True) - # setup variable list - var_list = self.weights.values() + # Setup variable list. + var_list = list(self.weights.values()) if finetune: var_list = [] - for weights_name, weights_val in self.weights.iteritems(): + for weights_name, weights_val in self.weights.items(): layer_name = weight_name_to_layer_name(weights_name) if self.optimize_base_layers or layer_name not in self.gqcnn._base_layer_names: var_list.append(weights_val) - # create optimizer - with tf.name_scope('optimizer'): + # Create optimizer. + with tf.name_scope("optimizer"): apply_grad_op, global_grad_norm = self._create_optimizer(loss, batch, var_list, learning_rate) - # add a handler for SIGINT for graceful exit + # Add a handler for SIGINT for graceful exit. def handler(signum, frame): - self.logger.info('caught CTRL+C, exiting...') + self.logger.info("caught CTRL+C, exiting...") self._cleanup() exit(0) signal.signal(signal.SIGINT, handler) - # now that everything in our graph is set up, we write the graph to the summary event so it can be visualized in tensorboard + # Now that everything in our graph is set up, we write the graph to the summary event so it can be visualized in Tensorboard. self.summary_writer.add_graph(self.gqcnn.tf_graph) - # begin optimization loop + # Begin optimization loop. try: - # start prefetch queue workers + # Start prefetch queue workers. self.prefetch_q_workers = [] seed = self._seed for i in range(self.num_prefetch_q_workers): @@ -318,19 +331,19 @@ def handler(signum, frame): p.start() self.prefetch_q_workers.append(p) - # init TF variables + # Init TF variables. init = tf.global_variables_initializer() self.sess.run(init) - self.logger.info('Beginning Optimization...') + self.logger.info("Beginning Optimization...") - # create a TrainStatsLogger object to log training statistics at certain intervals + # Create a `TrainStatsLogger` object to log training statistics at certain intervals. self.train_stats_logger = TrainStatsLogger(self.model_dir) - # loop through training steps + # Loop through training steps. training_range = xrange(int(self.num_epochs * self.num_train) // self.train_batch_size) for step in training_range: - # run optimization + # Run optimization. step_start = time.time() if self._angular_bins > 0: images, poses, labels, masks = self.prefetch_q.get() @@ -339,42 +352,42 @@ def handler(signum, frame): images, poses, labels = self.prefetch_q.get() _, l, ur_l, lr, predictions, raw_net_output = self.sess.run([apply_grad_op, loss, unregularized_loss, learning_rate, train_predictions, self.train_net_output], feed_dict={drop_rate_in: self.drop_rate, self.input_im_node: images, self.input_pose_node: poses, self.train_labels_node: labels}, options=GeneralConstants.timeout_option) step_stop = time.time() - self.logger.info('Step took %.3f sec.' %(step_stop-step_start)) + self.logger.info("Step took {} sec.".format(round(step_stop-step_start, 3))) if self.training_mode == TrainingMode.REGRESSION: - self.logger.info('Max ' + str(np.max(predictions))) - self.logger.info('Min ' + str(np.min(predictions))) - elif self.cfg['loss'] != 'weighted_cross_entropy': + self.logger.info("Max " + str(np.max(predictions))) + self.logger.info("Min " + str(np.min(predictions))) + elif self.cfg["loss"] != "weighted_cross_entropy": if self._angular_bins == 0: ex = np.exp(raw_net_output - np.tile(np.max(raw_net_output, axis=1)[:,np.newaxis], [1,2])) softmax = ex / np.tile(np.sum(ex, axis=1)[:,np.newaxis], [1,2]) - self.logger.info('Max ' + str(np.max(softmax[:,1]))) - self.logger.info('Min ' + str(np.min(softmax[:,1]))) - self.logger.info('Pred nonzero ' + str(np.sum(softmax[:,1] > 0.5))) - self.logger.info('True nonzero ' + str(np.sum(labels))) + self.logger.info("Max " + str(np.max(softmax[:,1]))) + self.logger.info("Min " + str(np.min(softmax[:,1]))) + self.logger.info("Pred nonzero " + str(np.sum(softmax[:,1] > 0.5))) + self.logger.info("True nonzero " + str(np.sum(labels))) else: sigmoid = 1.0 / (1.0 + np.exp(-raw_net_output)) - self.logger.info('Max ' + str(np.max(sigmoid))) - self.logger.info('Min ' + str(np.min(sigmoid))) - self.logger.info('Pred nonzero ' + str(np.sum(sigmoid > 0.5))) - self.logger.info('True nonzero ' + str(np.sum(labels > 0.5))) + self.logger.info("Max " + str(np.max(sigmoid))) + self.logger.info("Min " + str(np.min(sigmoid))) + self.logger.info("Pred nonzero " + str(np.sum(sigmoid > 0.5))) + self.logger.info("True nonzero " + str(np.sum(labels > 0.5))) if np.isnan(l) or np.any(np.isnan(poses)): - self.logger.error('Encountered NaN in loss or training poses!') + self.logger.error("Encountered NaN in loss or training poses!") raise Exception - # log output + # Log output. if step % self.log_frequency == 0: elapsed_time = time.time() - start_time start_time = time.time() - self.logger.info('Step %d (epoch %.2f), %.1f s' % - (step, float(step) * self.train_batch_size / self.num_train, - 1000 * elapsed_time / self.eval_frequency)) - self.logger.info('Minibatch loss: %.3f, learning rate: %.6f' % (l, lr)) + self.logger.info("Step {} (epoch {}), {} s".format( + step, round(step * self.train_batch_size / self.num_train, 3), + round(1000 * elapsed_time / self.eval_frequency, 2))) + self.logger.info("Minibatch loss: {}, learning rate: {}".format(round(l, 3), round(lr, 6))) if self.progress_dict is not None: - self.progress_dict['epoch'] = round(float(step) * self.train_batch_size / self.num_train, 2) + self.progress_dict["epoch"] = round(step * self.train_batch_size / self.num_train, 2) train_error = l if self.training_mode == TrainingMode.CLASSIFICATION: @@ -383,66 +396,66 @@ def handler(signum, frame): classification_result = BinaryClassificationResult(predictions[:,1], labels) train_error = classification_result.error_rate - self.logger.info('Minibatch error: %.3f' %(train_error)) + self.logger.info("Minibatch error: {}".format(round(train_error, 3))) self.summary_writer.add_summary(self.sess.run(self.merged_log_summaries, feed_dict={self.minibatch_error_placeholder: train_error, self.minibatch_loss_placeholder: l, self.learning_rate_placeholder: lr}), step) sys.stdout.flush() - # update the TrainStatsLogger + # Update the `TrainStatsLogger`. self.train_stats_logger.update(train_eval_iter=step, train_loss=l, train_error=train_error, total_train_error=None, val_eval_iter=None, val_error=None, learning_rate=lr) - # evaluate model + # Evaluate model. if step % self.eval_frequency == 0 and step > 0: - if self.cfg['eval_total_train_error']: + if self.cfg["eval_total_train_error"]: train_result = self._error_rate_in_batches(validation_set=False) - self.logger.info('Training error: %.3f' %(train_result.error_rate)) + self.logger.info("Training error: {}".format(round(train_result.error_rate, 3))) - # update the TrainStatsLogger and save + # Update the `TrainStatsLogger` and save. self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=train_result.error_rate, total_train_loss=train_result.cross_entropy_loss, val_eval_iter=None, val_error=None, learning_rate=None) self.train_stats_logger.log() if self.train_pct < 1.0: val_result = self._error_rate_in_batches() self.summary_writer.add_summary(self.sess.run(self.merged_eval_summaries, feed_dict={self.val_error_placeholder: val_result.error_rate}), step) - self.logger.info('Validation error: %.3f' %(val_result.error_rate)) - self.logger.info('Validation loss: %.3f' %(val_result.cross_entropy_loss)) + self.logger.info("Validation error: {}".format(round(val_result.error_rate, 3))) + self.logger.info("Validation loss: {}".format(round(val_result.cross_entropy_loss, 3))) sys.stdout.flush() - # update the TrainStatsLogger + # Update the `TrainStatsLogger`. if self.train_pct < 1.0: self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=None, val_eval_iter=step, val_loss=val_result.cross_entropy_loss, val_error=val_result.error_rate, learning_rate=None) else: self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=None, val_eval_iter=step, learning_rate=None) - # save everything! + # Save everything! self.train_stats_logger.log() - # save the model + # Save the model. if step % self.save_frequency == 0 and step > 0: - self.saver.save(self.sess, os.path.join(self.model_dir, 'model_%05d.ckpt' %(step))) - self.saver.save(self.sess, os.path.join(self.model_dir, 'model.ckpt')) + self.saver.save(self.sess, os.path.join(self.model_dir, "model_{}.ckpt".format(step))) + self.saver.save(self.sess, os.path.join(self.model_dir, "model.ckpt")) - # launch tensorboard only after the first iteration + # Launch tensorboard only after the first iteration. if not self.tensorboard_has_launched: self.tensorboard_has_launched = True self._launch_tensorboard() - # get final errors and flush the stdout pipeline + # Get final errors and flush the stdout pipeline. final_val_result = self._error_rate_in_batches() - self.logger.info('Final validation error: %.3f%%' %final_val_result.error_rate) - self.logger.info('Final validation loss: %.3f' %final_val_result.cross_entropy_loss) - if self.cfg['eval_total_train_error']: + self.logger.info("Final validation error: {}".format(round(final_val_result.error_rate, 3))) + self.logger.info("Final validation loss: {}".format(round(final_val_result.cross_entropy_loss, 3))) + if self.cfg["eval_total_train_error"]: final_train_result = self._error_rate_in_batches(validation_set=False) - self.logger.info('Final training error: {}'.format(final_train_result.error_rate)) - self.logger.info('Final training loss: {}'.format(final_train_result.cross_entropy_loss)) + self.logger.info("Final training error: {}".format(final_train_result.error_rate)) + self.logger.info("Final training loss: {}".format(final_train_result.cross_entropy_loss)) sys.stdout.flush() - # update the TrainStatsLogger + # Update the `TrainStatsLogger`. self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=None, val_eval_iter=step, val_loss=final_val_result.cross_entropy_loss, val_error=final_val_result.error_rate, learning_rate=None) - # log & save everything! + # Log & save everything! self.train_stats_logger.log() - self.saver.save(self.sess, os.path.join(self.model_dir, 'model.ckpt')) + self.saver.save(self.sess, os.path.join(self.model_dir, "model.ckpt")) except Exception as e: self._cleanup() @@ -451,16 +464,16 @@ def handler(signum, frame): self._cleanup() def _compute_data_metrics(self): - """ Calculate image mean, image std, pose mean, pose std, normalization params """ - # subsample tensors (for faster runtime) + """Calculate image mean, image std, pose mean, pose std, normalization params.""" + # Subsample tensors for faster runtime. random_file_indices = np.random.choice(self.num_tensors, size=self.num_random_files, replace=False) if self.gqcnn.input_depth_mode == InputDepthMode.POSE_STREAM: - # compute image stats - im_mean_filename = os.path.join(self.model_dir, 'im_mean.npy') - im_std_filename = os.path.join(self.model_dir, 'im_std.npy') + # Compute image stats. + im_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_MEAN) + im_std_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_STD) if os.path.exists(im_mean_filename) and os.path.exists(im_std_filename): self.im_mean = np.load(im_mean_filename) self.im_std = np.load(im_std_filename) @@ -468,12 +481,12 @@ def _compute_data_metrics(self): self.im_mean = 0 self.im_std = 0 - # compute mean - self.logger.info('Computing image mean') + # Compute mean. + self.logger.info("Computing image mean") num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to image mean estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to image mean estimate".format(k+1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: @@ -481,28 +494,28 @@ def _compute_data_metrics(self): num_summed += self.train_index_map[i].shape[0] * im_data.shape[1] * im_data.shape[2] self.im_mean = self.im_mean / num_summed - # compute std - self.logger.info('Computing image std') + # Compute std. + self.logger.info("Computing image std") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to image std estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to image std estimate".format(k+1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: self.im_std += np.sum((im_data[train_indices, ...] - self.im_mean)**2) self.im_std = np.sqrt(self.im_std / num_summed) - # save + # Save. np.save(im_mean_filename, self.im_mean) np.save(im_std_filename, self.im_std) - # update gqcnn + # Update GQ-CNN instance. self.gqcnn.set_im_mean(self.im_mean) self.gqcnn.set_im_std(self.im_std) - # compute pose stats - pose_mean_filename = os.path.join(self.model_dir, 'pose_mean.npy') - pose_std_filename = os.path.join(self.model_dir, 'pose_std.npy') + # Compute pose stats. + pose_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.POSE_MEAN) + pose_std_filename = os.path.join(self.model_dir, GQCNNFilenames.POSE_STD) if os.path.exists(pose_mean_filename) and os.path.exists(pose_std_filename): self.pose_mean = np.load(pose_mean_filename) self.pose_std = np.load(pose_std_filename) @@ -510,22 +523,22 @@ def _compute_data_metrics(self): self.pose_mean = np.zeros(self.raw_pose_shape) self.pose_std = np.zeros(self.raw_pose_shape) - # compute mean + # Compute mean. num_summed = 0 - self.logger.info('Computing pose mean') + self.logger.info("Computing pose mean") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to pose mean estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to pose mean estimate".format(k+1, random_file_indices.shape[0])) pose_data = self.dataset.tensor(self.pose_field_name, i).arr train_indices = self.train_index_map[i] if self.gripper_mode == GripperMode.SUCTION: rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]/2, + size=pose_data.shape[0]//2, replace=False) pose_data[rand_indices, 4] = -pose_data[rand_indices, 4] elif self.gripper_mode == GripperMode.LEGACY_SUCTION: rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]/2, + size=pose_data.shape[0]//2, replace=False) pose_data[rand_indices, 3] = -pose_data[rand_indices, 3] if train_indices.shape[0] > 0: @@ -535,21 +548,21 @@ def _compute_data_metrics(self): num_summed += pose_data.shape[0] self.pose_mean = self.pose_mean / num_summed - # compute std - self.logger.info('Computing pose std') + # Compute std. + self.logger.info("Computing pose std") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to pose std estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to pose std estimate".format(k+1, random_file_indices.shape[0])) pose_data = self.dataset.tensor(self.pose_field_name, i).arr train_indices = self.train_index_map[i] if self.gripper_mode == GripperMode.SUCTION: rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]/2, + size=pose_data.shape[0]//2, replace=False) pose_data[rand_indices, 4] = -pose_data[rand_indices, 4] elif self.gripper_mode == GripperMode.LEGACY_SUCTION: rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]/2, + size=pose_data.shape[0]//2, replace=False) pose_data[rand_indices, 3] = -pose_data[rand_indices, 3] if train_indices.shape[0] > 0: @@ -559,25 +572,25 @@ def _compute_data_metrics(self): self.pose_std = np.sqrt(self.pose_std / num_summed) self.pose_std[self.pose_std==0] = 1.0 - # save + # Save. self.pose_mean = read_pose_data(self.pose_mean, self.gripper_mode) self.pose_std = read_pose_data(self.pose_std, self.gripper_mode) np.save(pose_mean_filename, self.pose_mean) np.save(pose_std_filename, self.pose_std) - # update gqcnn + # Update GQ-CNN instance. self.gqcnn.set_pose_mean(self.pose_mean) self.gqcnn.set_pose_std(self.pose_std) - # check for invalid values + # Check for invalid values. if np.any(np.isnan(self.pose_mean)) or np.any(np.isnan(self.pose_std)): - self.logger.error('Pose mean or pose std is NaN! Check the input dataset') + self.logger.error("Pose mean or pose std is NaN! Check the input dataset") exit(0) elif self.gqcnn.input_depth_mode == InputDepthMode.SUB: - # compute (image - depth) stats - im_depth_sub_mean_filename = os.path.join(self.model_dir, 'im_depth_sub_mean.npy') - im_depth_sub_std_filename = os.path.join(self.model_dir, 'im_depth_sub_std.npy') + # Compute (image - depth) stats. + im_depth_sub_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_DEPTH_SUB_MEAN) + im_depth_sub_std_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_DEPTH_SUB_STD) if os.path.exists(im_depth_sub_mean_filename) and os.path.exists(im_depth_sub_std_filename): self.im_depth_sub_mean = np.load(im_depth_sub_mean_filename) self.im_depth_sub_std = np.load(im_depth_sub_std_filename) @@ -585,12 +598,12 @@ def _compute_data_metrics(self): self.im_depth_sub_mean = 0 self.im_depth_sub_std = 0 - # compute mean - self.logger.info('Computing (image - depth) mean') + # Compute mean. + self.logger.info("Computing (image - depth) mean") num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to (image - depth) mean estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to (image - depth) mean estimate".format(k+1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr depth_data = read_pose_data(self.dataset.tensor(self.pose_field_name, i).arr, self.gripper_mode) sub_data = im_data - np.tile(np.reshape(depth_data, (-1, 1, 1, 1)), (1, im_data.shape[1], im_data.shape[2], 1)) @@ -600,11 +613,11 @@ def _compute_data_metrics(self): num_summed += self.train_index_map[i].shape[0] * im_data.shape[1] * im_data.shape[2] self.im_depth_sub_mean = self.im_depth_sub_mean / num_summed - # compute std - self.logger.info('Computing (image - depth) std') + # Compute std. + self.logger.info("Computing (image - depth) std") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to (image - depth) std estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to (image - depth) std estimate".format(k+1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr depth_data = read_pose_data(self.dataset.tensor(self.pose_field_name, i).arr, self.gripper_mode) sub_data = im_data - np.tile(np.reshape(depth_data, (-1, 1, 1, 1)), (1, im_data.shape[1], im_data.shape[2], 1)) @@ -613,18 +626,18 @@ def _compute_data_metrics(self): self.im_depth_sub_std += np.sum((sub_data[train_indices, ...] - self.im_depth_sub_mean)**2) self.im_depth_sub_std = np.sqrt(self.im_depth_sub_std / num_summed) - # save + # Save. np.save(im_depth_sub_mean_filename, self.im_depth_sub_mean) np.save(im_depth_sub_std_filename, self.im_depth_sub_std) - # update gqcnn + # Update GQ-CNN instance. self.gqcnn.set_im_depth_sub_mean(self.im_depth_sub_mean) self.gqcnn.set_im_depth_sub_std(self.im_depth_sub_std) - elif self.gqcnn.input_depth_mode == InputDepthMode.IM_ONLY: - # compute image stats - im_mean_filename = os.path.join(self.model_dir, 'im_mean.npy') - im_std_filename = os.path.join(self.model_dir, 'im_std.npy') + elif self.gqcnn.input_depth_mode == InputDepthMode.IM_ONLY: + # Compute image stats. + im_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_MEAN) + im_std_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_STD) if os.path.exists(im_mean_filename) and os.path.exists(im_std_filename): self.im_mean = np.load(im_mean_filename) self.im_std = np.load(im_std_filename) @@ -632,12 +645,12 @@ def _compute_data_metrics(self): self.im_mean = 0 self.im_std = 0 - # compute mean - self.logger.info('Computing image mean') + # Compute mean. + self.logger.info("Computing image mean") num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to image mean estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to image mean estimate".format(k+1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: @@ -645,40 +658,40 @@ def _compute_data_metrics(self): num_summed += self.train_index_map[i].shape[0] * im_data.shape[1] * im_data.shape[2] self.im_mean = self.im_mean / num_summed - # compute std - self.logger.info('Computing image std') + # Compute std. + self.logger.info("Computing image std") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to image std estimate' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to image std estimate".format(k+1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: self.im_std += np.sum((im_data[train_indices, ...] - self.im_mean)**2) self.im_std = np.sqrt(self.im_std / num_summed) - # save + # Save. np.save(im_mean_filename, self.im_mean) np.save(im_std_filename, self.im_std) - # update gqcnn + # Update GQ-CNN instance. self.gqcnn.set_im_mean(self.im_mean) self.gqcnn.set_im_std(self.im_std) - # compute normalization parameters of the network - pct_pos_train_filename = os.path.join(self.model_dir, 'pct_pos_train.npy') - pct_pos_val_filename = os.path.join(self.model_dir, 'pct_pos_val.npy') + # Compute normalization parameters of the network. + pct_pos_train_filename = os.path.join(self.model_dir, GQCNNFilenames.PCT_POS_TRAIN) + pct_pos_val_filename = os.path.join(self.model_dir, GQCNNFilenames.PCT_POS_VAL) if os.path.exists(pct_pos_train_filename) and os.path.exists(pct_pos_val_filename): pct_pos_train = np.load(pct_pos_train_filename) pct_pos_val = np.load(pct_pos_val_filename) else: - self.logger.info('Computing grasp quality metric stats') + self.logger.info("Computing grasp quality metric stats") all_train_metrics = None all_val_metrics = None - # read metrics + # Read metrics. for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info('Adding file %d of %d to metric stat estimates' %(k+1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {} to metric stat estimates".format(k+1, random_file_indices.shape[0])) metric_data = self.dataset.tensor(self.label_field_name, i).arr train_indices = self.train_index_map[i] val_indices = self.val_index_map[i] @@ -697,26 +710,26 @@ def _compute_data_metrics(self): else: all_val_metrics = np.r_[all_val_metrics, val_metric_data] - # compute train stats + # Compute train stats. self.min_metric = np.min(all_train_metrics) self.max_metric = np.max(all_train_metrics) self.mean_metric = np.mean(all_train_metrics) self.median_metric = np.median(all_train_metrics) - # save metrics - pct_pos_train = float(np.sum(all_train_metrics > self.metric_thresh)) / all_train_metrics.shape[0] + # Save metrics. + pct_pos_train = np.sum(all_train_metrics > self.metric_thresh) / all_train_metrics.shape[0] np.save(pct_pos_train_filename, np.array(pct_pos_train)) if self.train_pct < 1.0: - pct_pos_val = float(np.sum(all_val_metrics > self.metric_thresh)) / all_val_metrics.shape[0] + pct_pos_val = np.sum(all_val_metrics > self.metric_thresh) / all_val_metrics.shape[0] np.save(pct_pos_val_filename, np.array(pct_pos_val)) - self.logger.info('Percent positive in train: ' + str(pct_pos_train)) + self.logger.info("Percent positive in train: " + str(pct_pos_train)) if self.train_pct < 1.0: - self.logger.info('Percent positive in val: ' + str(pct_pos_val)) + self.logger.info("Percent positive in val: " + str(pct_pos_val)) if self._angular_bins > 0: - self.logger.info('Calculating angular bin statistics...') + self.logger.info("Calculating angular bin statistics...") bin_counts = np.zeros((self._angular_bins,)) for m in range(self.num_tensors): pose_arr = self.dataset.tensor(self.pose_field_name, m).arr @@ -728,18 +741,19 @@ def _compute_data_metrics(self): l_neg_90 = np.where(angles < (-1 * (self._max_angle / 2))) angles[g_90] -= self._max_angle angles[l_neg_90] += self._max_angle - angles *= -1 # hack to fix reverse angle convention + angles *= -1 # Hack to fix reverse angle convention. + # TODO(vsatish): Actually fix this. angles += (self._max_angle / 2) for i in range(angles.shape[0]): bin_counts[int(angles[i] // self._bin_width)] += 1 - self.logger.info('Bin counts: {}'.format(bin_counts)) + self.logger.info("Bin counts: {}".format(bin_counts)) def _compute_split_indices(self): """ Compute train and validation indices for each tensor to speed data accesses """ - # read indices + # Read indices. train_indices, val_indices, _ = self.dataset.split(self.split_name) - # loop through tensors, assigning indices to each file + # Loop through tensors, assigning indices to each file. self.train_index_map = {} for i in range(self.dataset.num_tensors): self.train_index_map[i] = [] @@ -750,7 +764,7 @@ def _compute_split_indices(self): lowest = np.min(datapoint_indices) self.train_index_map[tensor_index].append(i - lowest) - for i, indices in self.train_index_map.iteritems(): + for i, indices in self.train_index_map.items(): self.train_index_map[i] = np.array(indices) self.val_index_map = {} @@ -759,208 +773,210 @@ def _compute_split_indices(self): for i in val_indices: tensor_index = self.dataset.tensor_index(i) - if tensor_index not in self.val_index_map.keys(): + if tensor_index not in self.val_index_map: self.val_index_map[tensor_index] = [] datapoint_indices = self.dataset.datapoint_indices_for_tensor(tensor_index) lowest = np.min(datapoint_indices) self.val_index_map[tensor_index].append(i - lowest) - for i, indices in self.val_index_map.iteritems(): + for i, indices in self.val_index_map.items(): self.val_index_map[i] = np.array(indices) def _setup_output_dirs(self): """Setup output directories.""" - self.logger.info('Saving model to: {}'.format(self.model_dir)) + self.logger.info("Saving model to: {}".format(self.model_dir)) - # create the summary dir - self.summary_dir = os.path.join(self.model_dir, 'tensorboard_summaries') + # Create the summary dir. + self.summary_dir = os.path.join(self.model_dir, "tensorboard_summaries") if not os.path.exists(self.summary_dir): os.mkdir(self.summary_dir) else: - # if the summary directory already exists, clean it out by deleting all files in it - # we don't want tensorboard to get confused with old logs while debugging with the same directory + # If the summary directory already exists, clean it out by deleting all files in it + # we don"t want tensorboard to get confused with old logs while debugging with the same directory. old_files = os.listdir(self.summary_dir) for f in old_files: os.remove(os.path.join(self.summary_dir, f)) - # setup filter directory - self.filter_dir = os.path.join(self.model_dir, 'filters') + # Setup filter directory. + self.filter_dir = os.path.join(self.model_dir, "filters") if not os.path.exists(self.filter_dir): os.mkdir(self.filter_dir) def _save_configs(self): """Save training configuration.""" - # update config for fine-tuning + # Update config for fine-tuning. if self.finetuning: - self.cfg['base_model_dir'] = self.base_model_dir + self.cfg["base_model_dir"] = self.base_model_dir - # save config - out_config_filename = os.path.join(self.model_dir, 'config.json') + # Save config. + out_config_filename = os.path.join(self.model_dir, "config.json") tempOrderedDict = collections.OrderedDict() - for key in self.cfg.keys(): + for key in self.cfg: tempOrderedDict[key] = self.cfg[key] - with open(out_config_filename, 'w') as outfile: + with open(out_config_filename, "w") as outfile: json.dump(tempOrderedDict, outfile, indent=GeneralConstants.JSON_INDENT) - # save training script + # Save training script. this_filename = sys.argv[0] - out_train_filename = os.path.join(self.model_dir, 'training_script.py') + out_train_filename = os.path.join(self.model_dir, "training_script.py") shutil.copyfile(this_filename, out_train_filename) - # save architecture - out_architecture_filename = os.path.join(self.model_dir, 'architecture.json') - json.dump(self.cfg['gqcnn']['architecture'], - open(out_architecture_filename, 'w'), + # Save architecture. + out_architecture_filename = os.path.join(self.model_dir, "architecture.json") + json.dump(self.cfg["gqcnn"]["architecture"], + open(out_architecture_filename, "w"), indent=GeneralConstants.JSON_INDENT) def _read_training_params(self): - """ Read training parameters from configuration file """ - # splits - self.train_pct = self.cfg['train_pct'] - self.total_pct = self.cfg['total_pct'] - - # training sizes - self.train_batch_size = self.cfg['train_batch_size'] - self.val_batch_size = self.cfg['val_batch_size'] + """Read training parameters from configuration file.""" + # Splits. + self.train_pct = self.cfg["train_pct"] + self.total_pct = self.cfg["total_pct"] + + # Training sizes. + self.train_batch_size = self.cfg["train_batch_size"] + self.val_batch_size = self.cfg["val_batch_size"] self.max_files_eval = None - if 'max_files_eval' in self.cfg.keys(): - self.max_files_eval = self.cfg['max_files_eval'] + if "max_files_eval" in self.cfg: + self.max_files_eval = self.cfg["max_files_eval"] - # logging - self.num_epochs = self.cfg['num_epochs'] - self.eval_frequency = self.cfg['eval_frequency'] - self.save_frequency = self.cfg['save_frequency'] - self.log_frequency = self.cfg['log_frequency'] - - # optimization - self.train_l2_regularizer = self.cfg['train_l2_regularizer'] - self.base_lr = self.cfg['base_lr'] - self.decay_step_multiplier = self.cfg['decay_step_multiplier'] - self.decay_rate = self.cfg['decay_rate'] - self.momentum_rate = self.cfg['momentum_rate'] - self.max_training_examples_per_load = self.cfg['max_training_examples_per_load'] - self.drop_rate = self.cfg['drop_rate'] - self.max_global_grad_norm = self.cfg['max_global_grad_norm'] + # Logging. + self.num_epochs = self.cfg["num_epochs"] + self.eval_frequency = self.cfg["eval_frequency"] + self.save_frequency = self.cfg["save_frequency"] + self.log_frequency = self.cfg["log_frequency"] + + # Optimization. + self.train_l2_regularizer = self.cfg["train_l2_regularizer"] + self.base_lr = self.cfg["base_lr"] + self.decay_step_multiplier = self.cfg["decay_step_multiplier"] + self.decay_rate = self.cfg["decay_rate"] + self.momentum_rate = self.cfg["momentum_rate"] + self.max_training_examples_per_load = self.cfg["max_training_examples_per_load"] + self.drop_rate = self.cfg["drop_rate"] + self.max_global_grad_norm = self.cfg["max_global_grad_norm"] self.optimize_base_layers = False - if 'optimize_base_layers' in self.cfg.keys(): - self.optimize_base_layers = self.cfg['optimize_base_layers'] + if "optimize_base_layers" in self.cfg: + self.optimize_base_layers = self.cfg["optimize_base_layers"] - # metrics - self.target_metric_name = self.cfg['target_metric_name'] - self.metric_thresh = self.cfg['metric_thresh'] - self.training_mode = self.cfg['training_mode'] + # Metrics. + self.target_metric_name = self.cfg["target_metric_name"] + self.metric_thresh = self.cfg["metric_thresh"] + self.training_mode = self.cfg["training_mode"] if self.training_mode != TrainingMode.CLASSIFICATION: - raise ValueError('Training mode %s not currently supported!' %(self.training_mode)) + raise ValueError("Training mode \"{}\" not currently supported!".format(self.training_mode)) - # tensorboad - self._tensorboard_port = self.cfg['tensorboard_port'] + # Tensorboad. + self._tensorboard_port = self.cfg["tensorboard_port"] - # preproc - self.preproc_log_frequency = self.cfg['preproc_log_frequency'] - self.num_random_files = self.cfg['num_random_files'] + # Preprocessing. + self.preproc_log_frequency = self.cfg["preproc_log_frequency"] + self.num_random_files = self.cfg["num_random_files"] self.max_prefetch_q_size = GeneralConstants.MAX_PREFETCH_Q_SIZE - if 'max_prefetch_q_size' in self.cfg.keys(): - self.max_prefetch_q_size = self.cfg['max_prefetch_q_size'] + if "max_prefetch_q_size" in self.cfg: + self.max_prefetch_q_size = self.cfg["max_prefetch_q_size"] self.num_prefetch_q_workers = GeneralConstants.NUM_PREFETCH_Q_WORKERS - if 'num_prefetch_q_workers' in self.cfg.keys(): - self.num_prefetch_q_workers = self.cfg['num_prefetch_q_workers'] + if "num_prefetch_q_workers" in self.cfg: + self.num_prefetch_q_workers = self.cfg["num_prefetch_q_workers"] - # re-weighting positives / negatives + # Re-weighting positives/negatives. self.pos_weight = 0.0 - if 'pos_weight' in self.cfg.keys(): - self.pos_weight = self.cfg['pos_weight'] + if "pos_weight" in self.cfg: + self.pos_weight = self.cfg["pos_weight"] self.pos_accept_prob = 1.0 self.neg_accept_prob = 1.0 if self.pos_weight > 1: - self.neg_accept_prob = 1.0 / self.pos_weight + self.neg_accept_prob = 1 / self.pos_weight else: self.pos_accept_prob = self.pos_weight if self.train_pct < 0 or self.train_pct > 1: - raise ValueError('Train percentage must be in range [0,1]') + raise ValueError("Train percentage must be in range [0,1]") if self.total_pct < 0 or self.total_pct > 1: - raise ValueError('Total percentage must be in range [0,1]') + raise ValueError("Total percentage must be in range [0,1]") - # normalization + # Input normalization. self._norm_inputs = True if self.gqcnn.input_depth_mode == InputDepthMode.SUB: self._norm_inputs = False - # angular training + # Angular training. self._angular_bins = self.gqcnn.angular_bins self._max_angle = self.gqcnn.max_angle - # during angular training, make sure symmetrization in denoising is turned off and also set the angular bin width + # During angular training, make sure symmetrization in denoising is + # turned off and also set the angular bin width. if self._angular_bins > 0: - assert not self.cfg['symmetrize'], 'Symmetrization denoising must be turned off during angular training' + assert not self.cfg["symmetrize"], "Symmetrization denoising must be turned off during angular training" self._bin_width = self._max_angle / self._angular_bins - # debugging - self._debug = self.cfg['debug'] - self._seed = self.cfg['seed'] + # Debugging. + self._debug = self.cfg["debug"] + self._seed = self.cfg["seed"] if self._debug: if self.num_prefetch_q_workers > 1: - self.logger.warning('Deterministic execution is not possible with ' - 'more than one prefetch queue worker even in debug mode.') - self.num_random_files = self.cfg['debug_num_files'] # this reduces initialization time + self.logger.warning("Deterministic execution is not possible with " + "more than one prefetch queue worker even in debug mode.") + self.num_random_files = self.cfg["debug_num_files"] # This reduces initialization time for fast debugging. np.random.seed(self._seed) random.seed(self._seed) def _setup_denoising_and_synthetic(self): - """ Setup denoising and synthetic data parameters """ - # multiplicative denoising - if self.cfg['multiplicative_denoising']: - self.gamma_shape = self.cfg['gamma_shape'] - self.gamma_scale = 1.0 / self.gamma_shape - # gaussian process noise - if self.cfg['gaussian_process_denoising']: - self.gp_rescale_factor = self.cfg['gaussian_process_scaling_factor'] + """Setup denoising and synthetic data parameters.""" + # Multiplicative denoising. + if self.cfg["multiplicative_denoising"]: + self.gamma_shape = self.cfg["gamma_shape"] + self.gamma_scale = 1 / self.gamma_shape + + # Gaussian process noise. + if self.cfg["gaussian_process_denoising"]: + self.gp_rescale_factor = self.cfg["gaussian_process_scaling_factor"] self.gp_sample_height = int(self.im_height / self.gp_rescale_factor) self.gp_sample_width = int(self.im_width / self.gp_rescale_factor) self.gp_num_pix = self.gp_sample_height * self.gp_sample_width - self.gp_sigma = self.cfg['gaussian_process_sigma'] + self.gp_sigma = self.cfg["gaussian_process_sigma"] def _open_dataset(self): - """ Open the dataset """ - # read in filenames of training data(poses, images, labels) + """Open the dataset.""" + # Read in filenames of training data (poses, images, labels). self.dataset = TensorDataset.open(self.dataset_dir) self.num_datapoints = self.dataset.num_datapoints self.num_tensors = self.dataset.num_tensors self.datapoints_per_file = self.dataset.datapoints_per_file self.num_random_files = min(self.num_tensors, self.num_random_files) - # read split + # Read split. if not self.dataset.has_split(self.split_name): - self.logger.info('Training split: {} not found in dataset. Creating new split...'.format(self.split_name)) + self.logger.info("Training split: {} not found in dataset. Creating new split...".format(self.split_name)) self.dataset.make_split(self.split_name, train_pct=self.train_pct) else: - self.logger.info('Training split: {} found in dataset.'.format(self.split_name)) + self.logger.info("Training split: {} found in dataset.".format(self.split_name)) self._compute_split_indices() def _compute_data_params(self): - """ Compute parameters of the dataset """ - # image params - self.im_field_name = self.cfg['image_field_name'] - self.im_height = self.dataset.config['fields'][self.im_field_name]['height'] - self.im_width = self.dataset.config['fields'][self.im_field_name]['width'] - self.im_channels = self.dataset.config['fields'][self.im_field_name]['channels'] - self.im_center = np.array([float(self.im_height-1)/2, float(self.im_width-1)/2]) - - # poses - self.pose_field_name = self.cfg['pose_field_name'] + """Compute parameters of the dataset.""" + # Image params. + self.im_field_name = self.cfg["image_field_name"] + self.im_height = self.dataset.config["fields"][self.im_field_name]["height"] + self.im_width = self.dataset.config["fields"][self.im_field_name]["width"] + self.im_channels = self.dataset.config["fields"][self.im_field_name]["channels"] + self.im_center = np.array([self.im_height // 2, self.im_width // 2]) # NOTE: There was originally some weird math going on here... + + # Poses. + self.pose_field_name = self.cfg["pose_field_name"] self.gripper_mode = self.gqcnn.gripper_mode self.pose_dim = pose_dim(self.gripper_mode) - self.raw_pose_shape = self.dataset.config['fields'][self.pose_field_name]['height'] + self.raw_pose_shape = self.dataset.config["fields"][self.pose_field_name]["height"] - # outputs + # Outputs. self.label_field_name = self.target_metric_name self.num_categories = 2 - # compute the number of train and val examples + # Compute the number of train and val examples. self.num_train = 0 self.num_val = 0 for train_indices in self.train_index_map.values(): @@ -968,95 +984,95 @@ def _compute_data_params(self): for val_indices in self.train_index_map.values(): self.num_val += val_indices.shape[0] - # set params based on the number of training examples (convert epochs to steps) - self.eval_frequency = int(np.ceil(self.eval_frequency * (float(self.num_train) / self.train_batch_size))) - self.save_frequency = int(np.ceil(self.save_frequency * (float(self.num_train) / self.train_batch_size))) + # Set params based on the number of training examples (convert epochs to steps). + self.eval_frequency = int(np.ceil(self.eval_frequency * (self.num_train / self.train_batch_size))) + self.save_frequency = int(np.ceil(self.save_frequency * (self.num_train / self.train_batch_size))) self.decay_step = self.decay_step_multiplier * self.num_train def _setup_tensorflow(self): - """Setup Tensorflow placeholders, session, and queue""" + """Setup Tensorflow placeholders, session, and queue.""" - # set up training label and numpy datatypes + # Set up training label and NumPy data types. if self.training_mode == TrainingMode.REGRESSION: train_label_dtype = tf.float32 self.numpy_dtype = np.float32 elif self.training_mode == TrainingMode.CLASSIFICATION: train_label_dtype = tf.int64 self.numpy_dtype = np.int64 - if self.cfg['loss'] == 'weighted_cross_entropy': + if self.cfg["loss"] == "weighted_cross_entropy": train_label_dtype = tf.float32 self.numpy_dtype = np.float32 else: - raise ValueError('Training mode %s not supported' %(self.training_mode)) + raise ValueError("Training mode \"{}\" not supported".format(self.training_mode)) - # set up placeholders + # Set up placeholders. self.train_labels_node = tf.placeholder(train_label_dtype, (self.train_batch_size,)) self.input_im_node = tf.placeholder(tf.float32, (self.train_batch_size, self.im_height, self.im_width, self.im_channels)) self.input_pose_node = tf.placeholder(tf.float32, (self.train_batch_size, self.pose_dim)) if self._angular_bins > 0: self.train_pred_mask_node = tf.placeholder(tf.int32, (self.train_batch_size, self._angular_bins * 2)) - # create data prefetch queue + # Create data prefetch queue. self.prefetch_q = mp.Queue(self.max_prefetch_q_size) - # get weights + # Get weights. self.weights = self.gqcnn.weights - # open a tf session for the gqcnn object and store it also as the optimizer session + # Open a TF session for the GQ-CNN instance and store it also as the optimizer session. self.sess = self.gqcnn.open_session() - # setup data prefetch queue worker termination event + # Setup data prefetch queue worker termination event. self.term_event = mp.Event() self.term_event.clear() def _setup_summaries(self): - """ Sets up placeholders for summary values and creates summary writer """ - # we create placeholders for our python values because summary_scalar expects - # a placeholder, not simply a python value + """Sets up placeholders for summary values and creates summary writer.""" + # Create placeholders for Python values because `tf.summary.scalar` expects + # a placeholder. self.val_error_placeholder = tf.placeholder(tf.float32, []) self.minibatch_error_placeholder = tf.placeholder(tf.float32, []) self.minibatch_loss_placeholder = tf.placeholder(tf.float32, []) self.learning_rate_placeholder = tf.placeholder(tf.float32, []) - # we create summary scalars with tags that allow us to group them together so we can write different batches - # of summaries at different intervals - tf.summary.scalar('val_error', self.val_error_placeholder, collections=["eval_frequency"]) - tf.summary.scalar('minibatch_error', self.minibatch_error_placeholder, collections=["log_frequency"]) - tf.summary.scalar('minibatch_loss', self.minibatch_loss_placeholder, collections=["log_frequency"]) - tf.summary.scalar('learning_rate', self.learning_rate_placeholder, collections=["log_frequency"]) + # Tag the `tf.summary.scalar`s so that we can group them together and + # write different batches of summaries at different intervals. + tf.summary.scalar("val_error", self.val_error_placeholder, collections=["eval_frequency"]) + tf.summary.scalar("minibatch_error", self.minibatch_error_placeholder, collections=["log_frequency"]) + tf.summary.scalar("minibatch_loss", self.minibatch_loss_placeholder, collections=["log_frequency"]) + tf.summary.scalar("learning_rate", self.learning_rate_placeholder, collections=["log_frequency"]) self.merged_eval_summaries = tf.summary.merge_all("eval_frequency") self.merged_log_summaries = tf.summary.merge_all("log_frequency") - # create a tf summary writer with the specified summary directory + # Create a TF summary writer with the specified summary directory. self.summary_writer = tf.summary.FileWriter(self.summary_dir) - # initialize the variables again now that we have added some new ones + # Initialize the variables again now that we have added some new ones. with self.sess.as_default(): tf.global_variables_initializer().run() def _cleanup(self): - self.logger.info('Cleaning and preparing to exit optimization...') + self.logger.info("Cleaning and preparing to exit optimization...") - # set termination even for prefetch queue workers - self.logger.info('Terminating prefetch queue workers...') + # Set termination event for prefetch queue workers. + self.logger.info("Terminating prefetch queue workers...") self.term_event.set() - # flush prefetch queue - #NOTE: this prevents a deadlock with the worker process queue buffers + # Flush prefetch queue. + # NOTE: This prevents a deadlock with the worker process queue buffers. self._flush_prefetch_queue() - # join prefetch queue worker processes + # Join prefetch queue worker processes. for p in self.prefetch_q_workers: p.join() - # close tensorboard if started + # Close tensorboard if started. if self.tensorboard_has_launched: self._close_tensorboard() - # close tensorflow session + # Close Tensorflow session. self.gqcnn.close_session() - # cleanup + # Free memory. for layer_weights in self.weights.values(): del layer_weights del self.saver @@ -1064,64 +1080,64 @@ def _cleanup(self): def _flush_prefetch_queue(self): """Flush prefetch queue.""" - self.logger.info('Flushing prefetch queue...') + self.logger.info("Flushing prefetch queue...") for i in range(self.prefetch_q.qsize()): self.prefetch_q.get() def _setup(self): """Setup for training.""" - # initialize data prefetch queue thread exit booleans + # Initialize data prefetch queue thread exit booleans. self.queue_thread_exited = False self.forceful_exit = False - # setup output directories + # Setup output directories. self._setup_output_dirs() - # save training configuration + # Save training configuration. self._save_configs() - # read training parameters from config file + # Read training parameters from config file. self._read_training_params() - # setup image and pose data files + # Setup image and pose data files. self._open_dataset() - # compute data parameters + # Compute data parameters. self._compute_data_params() - # setup denoising and synthetic data parameters + # Setup denoising and synthetic data parameters. self._setup_denoising_and_synthetic() - # compute means, std's, and normalization metrics + # Compute means, std's, and normalization metrics. self._compute_data_metrics() - # setup tensorflow session/placeholders/queue + # Setup Tensorflow session/placeholders/queue. self._setup_tensorflow() - # setup summaries for visualizing metrics in tensorboard + # Setup summaries for visualizing metrics in Tensorboard. self._setup_summaries() def _load_and_enqueue(self, seed): - """ Loads and enqueues a batch of images for training """ - signal.signal(signal.SIGINT, signal.SIG_IGN) # when the parent process receives a SIGINT, it will itself handle cleaning up child processes + """Loads and enqueues a batch of images for training.""" + signal.signal(signal.SIGINT, signal.SIG_IGN) # When the parent process receives a SIGINT, it will itself handle cleaning up child processes. - # set the random seed explicitly to prevent all workers from possible inheriting - # the same seed on process initialization + # Set the random seed explicitly to prevent all workers from possible inheriting + # the same seed on process initialization. np.random.seed(seed) random.seed(seed) - # open dataset + # Open dataset. dataset = TensorDataset.open(self.dataset_dir) while not self.term_event.is_set(): - # loop through data + # Loop through data. num_queued = 0 start_i = 0 end_i = 0 file_num = 0 queue_start = time.time() - # init buffers + # Init buffers. train_images = np.zeros( [self.train_batch_size, self.im_height, self.im_width, self.im_channels]).astype(np.float32) train_poses = np.zeros([self.train_batch_size, self.pose_dim]).astype(np.float32) @@ -1130,10 +1146,10 @@ def _load_and_enqueue(self, seed): train_pred_mask = np.zeros((self.train_batch_size, self._angular_bins*2), dtype=bool) while start_i < self.train_batch_size: - # compute num remaining + # Compute num remaining. num_remaining = self.train_batch_size - num_queued - # gen tensor index uniformly at random + # Generate tensor index uniformly at random. file_num = np.random.choice(self.num_tensors, size=1)[0] read_start = time.time() @@ -1141,17 +1157,17 @@ def _load_and_enqueue(self, seed): train_poses_tensor = dataset.tensor(self.pose_field_name, file_num) train_labels_tensor = dataset.tensor(self.label_field_name, file_num) read_stop = time.time() - self.logger.debug('Reading data took %.3f sec' %(read_stop - read_start)) - self.logger.debug('File num: %d' %(file_num)) + self.logger.debug("Reading data took {} sec".format(round(read_stop - read_start, 3))) + self.logger.debug("File num: {}".format(file_num)) - # get batch indices uniformly at random + # Get batch indices uniformly at random. train_ind = self.train_index_map[file_num] np.random.shuffle(train_ind) if self.gripper_mode == GripperMode.LEGACY_SUCTION: tp_tmp = read_pose_data(train_poses_tensor.data, self.gripper_mode) train_ind = train_ind[np.isfinite(tp_tmp[train_ind,1])] - # filter positives and negatives + # Filter positives and negatives. if self.training_mode == TrainingMode.CLASSIFICATION and self.pos_weight != 0.0: labels = 1 * (train_labels_tensor.arr > self.metric_thresh) np.random.shuffle(train_ind) @@ -1163,23 +1179,23 @@ def _load_and_enqueue(self, seed): filtered_ind.append(index) train_ind = np.array(filtered_ind) - # samples train indices + # Samples train indices. upper = min(num_remaining, train_ind.shape[0], self.max_training_examples_per_load) ind = train_ind[:upper] num_loaded = ind.shape[0] if num_loaded == 0: - self.logger.warning('Queueing zero examples!!!!') + self.logger.warning("Queueing zero examples!!!!") continue - # subsample data + # Subsample data. train_images_arr = train_images_tensor.arr[ind, ...] train_poses_arr = train_poses_tensor.arr[ind, ...] angles = train_poses_arr[:, 3] train_label_arr = train_labels_tensor.arr[ind] num_images = train_images_arr.shape[0] - # resize images - rescale_factor = float(self.im_height) / train_images_arr.shape[1] + # Resize images. + rescale_factor = self.im_height / train_images_arr.shape[1] if rescale_factor != 1.0: resized_train_images_arr = np.zeros([num_images, self.im_height, @@ -1189,17 +1205,17 @@ def _load_and_enqueue(self, seed): for c in range(train_images_arr.shape[3]): resized_train_images_arr[i,:,:,c] = sm.imresize(train_images_arr[i,:,:,c], rescale_factor, - interp='bicubic', mode='F') + interp="bicubic", mode="F") train_images_arr = resized_train_images_arr - # add noises to images + # Add noises to images. train_images_arr, train_poses_arr = self._distort(train_images_arr, train_poses_arr) - # slice poses + # Slice poses. train_poses_arr = read_pose_data(train_poses_arr, self.gripper_mode) - # standardize inputs and outputs + # Standardize inputs and outputs. if self._norm_inputs: train_images_arr = (train_images_arr - self.im_mean) / self.im_std if self.gqcnn.input_depth_mode == InputDepthMode.POSE_STREAM: @@ -1209,7 +1225,7 @@ def _load_and_enqueue(self, seed): if self._angular_bins > 0: bins = np.zeros_like(train_label_arr) - # form prediction mask to use when calculating loss + # Form prediction mask to use when calculating loss. neg_ind = np.where(angles < 0) angles = np.abs(angles) % self._max_angle angles[neg_ind] *= -1 @@ -1217,7 +1233,8 @@ def _load_and_enqueue(self, seed): l_neg_90 = np.where(angles < (-1 * (self._max_angle / 2))) angles[g_90] -= self._max_angle angles[l_neg_90] += self._max_angle - angles *= -1 # hack to fix reverse angle convention + angles *= -1 # Hack to fix reverse angle convention. + # TODO(vsatish): Actually fix this. angles += (self._max_angle / 2) train_pred_mask_arr = np.zeros((train_label_arr.shape[0], self._angular_bins*2)) for i in range(angles.shape[0]): @@ -1225,11 +1242,11 @@ def _load_and_enqueue(self, seed): train_pred_mask_arr[i, int((angles[i] // self._bin_width)*2)] = 1 train_pred_mask_arr[i, int((angles[i] // self._bin_width)*2 + 1)] = 1 - # compute the number of examples loaded + # Compute the number of examples loaded. num_loaded = train_images_arr.shape[0] end_i = start_i + num_loaded - # enqueue training data batch + # Enqueue training data batch. train_images[start_i:end_i, ...] = train_images_arr.copy() train_poses[start_i:end_i,:] = train_poses_arr.copy() train_labels[start_i:end_i] = train_label_arr.copy() @@ -1240,11 +1257,11 @@ def _load_and_enqueue(self, seed): del train_poses_arr del train_label_arr - # update start index + # Update start index. start_i = end_i num_queued += num_loaded - # send data to queue + # Send data to queue. if not self.term_event.is_set(): try: if self._angular_bins > 0: @@ -1254,7 +1271,7 @@ def _load_and_enqueue(self, seed): except Queue.Full: time.sleep(GeneralConstants.QUEUE_SLEEP) queue_stop = time.time() - self.logger.debug('Queue batch took %.3f sec' %(queue_stop - queue_start)) + self.logger.debug("Queue batch took {} sec".format(round(queue_stop - queue_start, 3))) del train_images del train_poses del train_labels @@ -1262,31 +1279,31 @@ def _load_and_enqueue(self, seed): del train_pred_mask def _distort(self, image_arr, pose_arr): - """ Adds noise to a batch of images """ - # read params + """Adds noise to a batch of images.""" + # Read params. num_images = image_arr.shape[0] - # denoising and synthetic data generation - if self.cfg['multiplicative_denoising']: + # Denoising and synthetic data generation. + if self.cfg["multiplicative_denoising"]: mult_samples = ss.gamma.rvs(self.gamma_shape, scale=self.gamma_scale, size=num_images) mult_samples = mult_samples[:,np.newaxis,np.newaxis,np.newaxis] image_arr = image_arr * np.tile(mult_samples, [1, self.im_height, self.im_width, self.im_channels]) - # add correlated Gaussian noise - if self.cfg['gaussian_process_denoising']: + # Add correlated Gaussian noise. + if self.cfg["gaussian_process_denoising"]: for i in range(num_images): - if np.random.rand() < self.cfg['gaussian_process_rate']: + if np.random.rand() < self.cfg["gaussian_process_rate"]: train_image = image_arr[i,:,:,0] gp_noise = ss.norm.rvs(scale=self.gp_sigma, size=self.gp_num_pix).reshape(self.gp_sample_height, self.gp_sample_width) - gp_noise = sm.imresize(gp_noise, self.gp_rescale_factor, interp='bicubic', mode='F') + gp_noise = sm.imresize(gp_noise, self.gp_rescale_factor, interp="bicubic", mode="F") train_image[train_image > 0] += gp_noise[train_image > 0] image_arr[i,:,:,0] = train_image - # symmetrize images - if self.cfg['symmetrize']: + # Symmetrize images. + if self.cfg["symmetrize"]: for i in range(num_images): train_image = image_arr[i,:,:,0] - # rotate with 50% probability + # Rotate with 50% probability. if np.random.rand() < 0.5: theta = 180.0 rot_map = cv2.getRotationMatrix2D(tuple(self.im_center), theta, 1) @@ -1296,10 +1313,10 @@ def _distort(self, image_arr, pose_arr): pose_arr[:,3] = -pose_arr[:,3] elif self.gripper_mode == GripperMode.SUCTION: pose_arr[:,4] = -pose_arr[:,4] - # reflect left right with 50% probability + # Reflect left right with 50% probability. if np.random.rand() < 0.5: train_image = np.fliplr(train_image) - # reflect up down with 50% probability + # Reflect up down with 50% probability. if np.random.rand() < 0.5: train_image = np.flipud(train_image) @@ -1311,17 +1328,17 @@ def _distort(self, image_arr, pose_arr): return image_arr, pose_arr def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): - """ Compute error and loss over either training or validation set + """Compute error and loss over either training or validation set. Returns ------- - :obj:'autolab_core.BinaryClassificationResult` + :obj:"autolab_core.BinaryClassificationResult` validation error """ all_predictions = [] all_labels = [] - # subsample files + # Subsample files. file_indices = np.arange(self.num_tensors) if num_files_eval is None: num_files_eval = self.max_files_eval @@ -1330,13 +1347,13 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): file_indices = file_indices[:num_files_eval] for i in file_indices: - # load next file + # Load next file. images = self.dataset.tensor(self.im_field_name, i).arr poses = self.dataset.tensor(self.pose_field_name, i).arr raw_poses = np.array(poses, copy=True) labels = self.dataset.tensor(self.label_field_name, i).arr - # if no datapoints from this file are in validation then just continue + # If no datapoints from this file are in validation then just continue. if validation_set: indices = self.val_index_map[i] else: @@ -1355,7 +1372,7 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): labels = labels.astype(np.uint8) if self._angular_bins > 0: - # form mask to extract predictions from ground-truth angular bins + # Form mask to extract predictions from ground-truth angular bins. angles = raw_poses[:, 3] neg_ind = np.where(angles < 0) angles = np.abs(angles) % self._max_angle @@ -1364,28 +1381,29 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): l_neg_90 = np.where(angles < (-1 * (self._max_angle / 2))) angles[g_90] -= self._max_angle angles[l_neg_90] += self._max_angle - angles *= -1 # hack to fix reverse angle convention + angles *= -1 # Hack to fix reverse angle convention. + # TODO(vsatish): Actually fix this. angles += (self._max_angle / 2) pred_mask = np.zeros((labels.shape[0], self._angular_bins*2), dtype=bool) for i in range(angles.shape[0]): pred_mask[i, int((angles[i] // self._bin_width)*2)] = True pred_mask[i, int((angles[i] // self._bin_width)*2 + 1)] = True - # get predictions + # Get predictions. predictions = self.gqcnn.predict(images, poses) if self._angular_bins > 0: predictions = predictions[pred_mask].reshape((-1, 2)) - # update + # Update. all_predictions.extend(predictions[:,1].tolist()) all_labels.extend(labels.tolist()) - # clean up + # Clean up. del images del poses - # get learning result + # Get learning result. result = None if self.training_mode == TrainingMode.CLASSIFICATION: result = BinaryClassificationResult(all_predictions, all_labels) diff --git a/gqcnn/utils/__init__.py b/gqcnn/utils/__init__.py index 04b8228d..989d9d25 100644 --- a/gqcnn/utils/__init__.py +++ b/gqcnn/utils/__init__.py @@ -1,10 +1,43 @@ -from utils import set_cuda_visible_devices, pose_dim, read_pose_data, reduce_shape, weight_name_to_layer_name -from enums import ImageMode, TrainingMode, GripperMode, InputDepthMode, GeneralConstants, GQCNNTrainingStatus -from policy_exceptions import NoValidGraspsException, NoAntipodalPairsFoundException -from train_stats_logger import TrainStatsLogger - -__all__ = ['set_cuda_visible_devices', 'pose_dim', 'read_pose_data', 'reduce_shape', - 'weight_name_to_layer_name', 'ImageMode', 'TrainingMode', - 'GripperMode', 'InputDepthMode', 'GeneralConstants', 'GQCNNTrainingStatus', - 'NoValidGraspsException', 'NoAntipodalPairsFoundException', - 'TrainStatsLogger'] +# -*- coding: utf-8 -*- +""" +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. +""" +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +from .enums import (ImageMode, TrainingMode, GripperMode, + InputDepthMode, GeneralConstants, + GQCNNTrainingStatus) +from .policy_exceptions import (NoValidGraspsException, + NoAntipodalPairsFoundException) +from .train_stats_logger import TrainStatsLogger +from .utils import (set_cuda_visible_devices, pose_dim, + read_pose_data, reduce_shape, + weight_name_to_layer_name) + +__all__ = ["set_cuda_visible_devices", "pose_dim", "read_pose_data", "reduce_shape", + "weight_name_to_layer_name", "ImageMode", "TrainingMode", + "GripperMode", "InputDepthMode", "GeneralConstants", "GQCNNTrainingStatus", + "NoValidGraspsException", "NoAntipodalPairsFoundException", + "TrainStatsLogger"] diff --git a/gqcnn/utils/enums.py b/gqcnn/utils/enums.py index 28524e57..25f1d252 100644 --- a/gqcnn/utils/enums.py +++ b/gqcnn/utils/enums.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,57 +21,84 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Constants/enums. Author: Vishal Satish """ -import tensorflow as tf +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import math -# other constants -class GeneralConstants: +import tensorflow as tf + +# Other constants. +class GeneralConstants(object): SEED = 3472134 - SEED_SAMPLE_MAX = 2**32 - 1 # max range for seed (at least for np.random.seed) + SEED_SAMPLE_MAX = 2**32 - 1 # Max range for seed (at least for `np.random.seed`). timeout_option = tf.RunOptions(timeout_in_ms=1000000) JSON_INDENT = 2 MAX_PREFETCH_Q_SIZE = 250 NUM_PREFETCH_Q_WORKERS = 3 QUEUE_SLEEP = 0.001 PI = math.pi + FIGSIZE = 16 # For display. -# enum for image modalities -class ImageMode: - BINARY = 'binary' - DEPTH = 'depth' - BINARY_TF = 'binary_tf' - COLOR_TF = 'color_tf' - GRAY_TF = 'gray_tf' - DEPTH_TF = 'depth_tf' - DEPTH_TF_TABLE = 'depth_tf_table' - TF_DEPTH_IMS = 'tf_depth_ims' +# Enum for image modalities. +class ImageMode(object): + BINARY = "binary" + DEPTH = "depth" + BINARY_TF = "binary_tf" + COLOR_TF = "color_tf" + GRAY_TF = "gray_tf" + DEPTH_TF = "depth_tf" + DEPTH_TF_TABLE = "depth_tf_table" + TF_DEPTH_IMS = "tf_depth_ims" -# enum for training modes -class TrainingMode: - CLASSIFICATION = 'classification' - REGRESSION = 'regression' # has not been shown to work, for experimentation only! +# Enum for training modes. +class TrainingMode(object): + CLASSIFICATION = "classification" + REGRESSION = "regression" # Has not been shown to work, for experimentation only! + +# Enum for input pose data formats. +class GripperMode(object): + PARALLEL_JAW = "parallel_jaw" + SUCTION = "suction" + MULTI_SUCTION = "multi_suction" + LEGACY_PARALLEL_JAW = "legacy_parallel_jaw" + LEGACY_SUCTION = "legacy_suction" + +# Enum for input depth mode. +class InputDepthMode(object): + POSE_STREAM = "pose_stream" + SUB = "im_depth_sub" + IM_ONLY = "im_only" + +# Enum for training status. +class GQCNNTrainingStatus(object): + NOT_STARTED = "not_started" + SETTING_UP = "setting_up" + TRAINING = "training" + +# Enum for filenames. +class GQCNNFilenames(object): + PCT_POS_VAL = "pct_pos_val.npy" + LEARNING_RATES = "learning_rates.npy" -# enum for input pose data formats -class GripperMode: - PARALLEL_JAW = 'parallel_jaw' - SUCTION = 'suction' - MULTI_SUCTION = 'multi_suction' - LEGACY_PARALLEL_JAW = 'legacy_parallel_jaw' - LEGACY_SUCTION = 'legacy_suction' + TRAIN_ITERS = "train_eval_iters.npy" + TRAIN_LOSSES = "train_losses.npy" + TRAIN_ERRORS = "train_errors.npy" + TOTAL_TRAIN_LOSSES = "total_train_losses.npy" + TOTAL_TRAIN_ERRORS = "total_train_errors.npy" -# enum for input depth mode -class InputDepthMode: - POSE_STREAM = 'pose_stream' - SUB = 'im_depth_sub' - IM_ONLY = 'im_only' + VAL_ITERS = "val_eval_iters.npy" + VAL_LOSSES = "val_losses.npy" + VAL_ERRORS = "val_errors.npy" -# enum for training status -class GQCNNTrainingStatus: - NOT_STARTED = 'not_started' - SETTING_UP = 'setting_up' - TRAINING = 'training' + IM_MEAN = "im_mean.npy" + IM_STD = "im_std.npy" + IM_DEPTH_SUB_MEAN = "im_depth_sub_mean.npy" + IM_DEPTH_SUB_STD = "im_depth_sub_std.npy" + POSE_MEAN = "pose_mean.npy" + POSE_STD = "pose_std.npy" diff --git a/gqcnn/utils/policy_exceptions.py b/gqcnn/utils/policy_exceptions.py index cb38ab0e..8aa7aff2 100644 --- a/gqcnn/utils/policy_exceptions.py +++ b/gqcnn/utils/policy_exceptions.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,19 +21,23 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Exceptions that can be thrown by sub-classes of GraspingPolicy. Author: Vishal Satish """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function class NoValidGraspsException(Exception): - """Exception for when antipodal point pairs can be found in the depth image but none are valid grasps that can be executed.""" + """Exception for when antipodal point pairs can be found in the depth + image but none are valid grasps that can be executed.""" def __init__(self, in_collision=True, not_confident=False, *args, **kwargs): self.in_collision = in_collision self.not_confident = not_confident Exception.__init__(self, *args, **kwargs) class NoAntipodalPairsFoundException(Exception): - """Exception for when no antipodal point pairs can be found in the depth image.""" + """Exception for when no antipodal point pairs can be found in the depth + image.""" pass diff --git a/gqcnn/utils/train_stats_logger.py b/gqcnn/utils/train_stats_logger.py index bf3dd25d..6573edeb 100644 --- a/gqcnn/utils/train_stats_logger.py +++ b/gqcnn/utils/train_stats_logger.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,14 +21,20 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Handles logging of various training statistics. Author: Vishal Satish """ -import numpy as np +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import os +import numpy as np + +from .enums import GQCNNFilenames + class TrainStatsLogger(object): """Logger for training statistics.""" @@ -53,90 +62,86 @@ def log(self): np.save( os.path.join( self.experiment_dir, - 'train_eval_iters.npy'), + GQCNNFilenames.TRAIN_ITERS), self.train_eval_iters) np.save( os.path.join( self.experiment_dir, - 'train_losses.npy'), + GQCNNFilenames.TRAIN_LOSSES), self.train_losses) np.save( os.path.join( self.experiment_dir, - 'train_errors.npy'), + GQCNNFilenames.TRAIN_ERRORS), self.train_errors) np.save( os.path.join( self.experiment_dir, - 'total_train_errors.npy'), + GQCNNFilenames.TOTAL_TRAIN_ERRORS), self.total_train_errors) np.save( os.path.join( self.experiment_dir, - 'total_train_losses.npy'), + GQCNNFilenames.TOTAL_TRAIN_LOSSES), self.total_train_losses) np.save( os.path.join( self.experiment_dir, - 'val_eval_iters.npy'), + GQCNNFilenames.VAL_ITERS), self.val_eval_iters) np.save( os.path.join( self.experiment_dir, - 'val_losses.npy'), + GQCNNFilenames.VAL_LOSSES), self.val_losses) np.save( os.path.join( self.experiment_dir, - 'val_errors.npy'), + GQCNNFilenames.VAL_ERRORS), self.val_errors) np.save( os.path.join( self.experiment_dir, - 'val_losses.npy'), - self.val_losses) - np.save( - os.path.join( - self.experiment_dir, - 'learning_rates.npy'), + GQCNNFilenames.LEARNING_RATES), self.learning_rates) def update(self, **stats): - """Update training statistics. NOTE: Any statistic that is None in the argument dict will not be updated. + """Update training statistics. + + Note + ---- + Any statistic that is `None` in the argument dict will not be updated. Parameters ---------- stats : dict dict of statistics to be updated """ - for statistic in stats: - if statistic == "train_eval_iter": - if stats[statistic] is not None: - self.train_eval_iters.append(stats[statistic]) - elif statistic == "train_loss": - if stats[statistic] is not None: - self.train_losses.append(stats[statistic]) - elif statistic == "train_error": - if stats[statistic] is not None: - self.train_errors.append(stats[statistic]) - elif statistic == "total_train_error": - if stats[statistic] is not None: - self.total_train_errors.append(stats[statistic]) - elif statistic == "total_train_loss": - if stats[statistic] is not None: - self.total_train_losses.append(stats[statistic]) - elif statistic == "val_eval_iter": - if stats[statistic] is not None: - self.val_eval_iters.append(stats[statistic]) - elif statistic == "val_loss": - if stats[statistic] is not None: - self.val_losses.append(stats[statistic]) - elif statistic == "val_error": - if stats[statistic] is not None: - self.val_errors.append(stats[statistic]) - elif statistic == "val_loss": - if stats[statistic] is not None: - self.val_losses.append(stats[statistic]) - elif statistic == "learning_rate": - if stats[statistic] is not None: - self.learning_rates.append(stats[statistic]) + for stat, val in stats.items(): + if stat == "train_eval_iter": + if val is not None: + self.train_eval_iters.append(val) + elif stat == "train_loss": + if val is not None: + self.train_losses.append(val) + elif stat == "train_error": + if val is not None: + self.train_errors.append(val) + elif stat == "total_train_error": + if val is not None: + self.total_train_errors.append(val) + elif stat == "total_train_loss": + if val is not None: + self.total_train_losses.append(val) + elif stat == "val_eval_iter": + if val is not None: + self.val_eval_iters.append(val) + elif stat == "val_loss": + if val is not None: + self.val_losses.append(val) + elif stat == "val_error": + if val is not None: + self.val_errors.append(val) + elif stat == "learning_rate": + if val is not None: + self.learning_rates.append(val) diff --git a/gqcnn/utils/utils.py b/gqcnn/utils/utils.py index e26b451e..af4ef073 100644 --- a/gqcnn/utils/utils.py +++ b/gqcnn/utils/utils.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,29 +21,38 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Simple utility functions Authors: Jeff Mahler, Vishal Satish, Lucas Manuelli """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +from functools import reduce import os import numpy as np from autolab_core import Logger -from enums import GripperMode +from .enums import GripperMode -# set up logger -logger = Logger.get_logger('gqcnn/utils/utils.py') +# Set up logger. +logger = Logger.get_logger("gqcnn/utils/utils.py") def set_cuda_visible_devices(gpu_list): - """ - Sets CUDA_VISIBLE_DEVICES environment variable to only show certain gpus + """Sets CUDA_VISIBLE_DEVICES environment variable to only show certain + gpus. + + Note + ---- If gpu_list is empty does nothing - :param gpu_list: list of gpus to set as visible - :return: None - """ + Parameters + ---------- + gpu_list : list + list of gpus to set as visible + """ if len(gpu_list) == 0: return @@ -52,8 +64,8 @@ def set_cuda_visible_devices(gpu_list): os.environ["CUDA_VISIBLE_DEVICES"] = cuda_visible_devices def pose_dim(gripper_mode): - """ Returns the dimensions of the pose vector for the given - gripper mode. + """Returns the dimensions of the pose vector for the given + gripper mode. Parameters ---------- @@ -77,10 +89,10 @@ def pose_dim(gripper_mode): elif gripper_mode == GripperMode.LEGACY_SUCTION: return 2 else: - raise ValueError('Gripper mode %s not supported.' %(gripper_mode)) + raise ValueError("Gripper mode \"{}\" not supported.".format(gripper_mode)) def read_pose_data(pose_arr, gripper_mode): - """ Read the pose data and slice it according to the specified gripper_mode + """Read the pose data and slice it according to the specified gripper mode. Parameters ---------- @@ -121,28 +133,27 @@ def read_pose_data(pose_arr, gripper_mode): else: return pose_arr[:,2:4] else: - raise ValueError('Gripper mode %s not supported.' %(gripper_mode)) + raise ValueError("Gripper mode %s not supported." %(gripper_mode)) def reduce_shape(shape): - """ Get shape of a layer for flattening """ + """Get shape of a layer for flattening.""" shape = [x.value for x in shape[1:]] f = lambda x, y: 1 if y is None else x * y return reduce(f, shape, 1) def weight_name_to_layer_name(weight_name): - """ Convert the name of weights to the layer name """ - tokens = weight_name.split('_') + """Convert the name of weights to the layer name.""" + tokens = weight_name.split("_") type_name = tokens[-1] - # modern naming convention - if type_name == 'weights' or type_name == 'bias': - if len(tokens) >= 3 and tokens[-3] == 'input': - return weight_name[:weight_name.rfind('input')-1] + # Modern naming convention. + if type_name == "weights" or type_name == "bias": + if len(tokens) >= 3 and tokens[-3] == "input": + return weight_name[:weight_name.rfind("input")-1] return weight_name[:weight_name.rfind(type_name)-1] - # legacy - if type_name == 'im': + # Legacy. + if type_name == "im": return weight_name[:-4] - if type_name == 'pose': + if type_name == "pose": return weight_name[:-6] return weight_name[:-1] - diff --git a/gqcnn/version.py b/gqcnn/version.py index 8af68578..4d3c61b0 100644 --- a/gqcnn/version.py +++ b/gqcnn/version.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,4 +22,8 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -__version__ = '1.0.0' +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +__version__ = "1.0.0" diff --git a/requirements/cpu_requirements.txt b/requirements/cpu_requirements.txt index cd6a8ea9..e6886666 100644 --- a/requirements/cpu_requirements.txt +++ b/requirements/cpu_requirements.txt @@ -1,12 +1,12 @@ autolab-core autolab-perception visualization -numpy>=1.14.0 +numpy opencv-python scipy -matplotlib<3.0.0 +matplotlib tensorflow>=1.10.0,<=1.13.1 scikit-learn -scikit-image<0.15.0 +scikit-image gputil psutil diff --git a/requirements/gpu_requirements.txt b/requirements/gpu_requirements.txt index a82b3d4c..e8b37f09 100644 --- a/requirements/gpu_requirements.txt +++ b/requirements/gpu_requirements.txt @@ -1,12 +1,12 @@ autolab-core autolab-perception visualization -numpy>=1.14.0 +numpy opencv-python scipy -matplotlib<3.0.0 +matplotlib tensorflow-gpu>=1.10.0,<=1.13.1 scikit-learn -scikit-image<0.15.0 +scikit-image gputil psutil diff --git a/setup.py b/setup.py index 46a98cc2..1b206b41 100644 --- a/setup.py +++ b/setup.py @@ -114,11 +114,11 @@ def run(self): 'autolab-core', 'autolab-perception', 'visualization', - 'numpy>=1.14.0', + 'numpy', 'scipy', - 'matplotlib<3.0.0', + 'matplotlib', 'opencv-python', - 'scikit-image<0.15.0', + 'scikit-image', 'scikit-learn', 'psutil', 'gputil' @@ -136,7 +136,8 @@ def run(self): keywords = 'robotics grasping vision deep learning', classifiers = [ 'Development Status :: 4 - Beta', - 'Programming Language :: Python :: 2.7 :: Only', + 'Programming Language :: Python :: 2.7', + 'Programming Language :: Python :: 3', 'Natural Language :: English', 'Topic :: Scientific/Engineering' ], From 1dc6cbee1471c36ec0f57560e2a4a3a809092f0c Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sat, 25 May 2019 01:24:25 -0700 Subject: [PATCH 02/29] WIP Refactoring. --- gqcnn/analysis/__init__.py | 24 +- gqcnn/analysis/analyzer.py | 383 +++++++++++++++-------------- gqcnn/grasping/policy/__init__.py | 10 +- gqcnn/grasping/policy/fc_policy.py | 7 +- gqcnn/grasping/policy/policy.py | 4 +- gqcnn/model/tf/fc_network_tf.py | 6 +- gqcnn/model/tf/network_tf.py | 34 +-- gqcnn/search/resource_manager.py | 2 +- gqcnn/search/search.py | 8 +- gqcnn/search/utils.py | 2 +- gqcnn/training/tf/trainer_tf.py | 18 +- gqcnn/utils/__init__.py | 13 +- gqcnn/utils/enums.py | 12 +- gqcnn/utils/utils.py | 6 +- 14 files changed, 281 insertions(+), 248 deletions(-) diff --git a/gqcnn/analysis/__init__.py b/gqcnn/analysis/__init__.py index 75323fc4..9dff9c2a 100644 --- a/gqcnn/analysis/__init__.py +++ b/gqcnn/analysis/__init__.py @@ -1,11 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, ishereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,6 +22,10 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. """ -from analyzer import GQCNNAnalyzer +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function -__all__ = ['GQCNNAnalyzer'] +from .analyzer import GQCNNAnalyzer + +__all__ = ["GQCNNAnalyzer"] diff --git a/gqcnn/analysis/analyzer.py b/gqcnn/analysis/analyzer.py index a1bb1aaa..1d7bced9 100644 --- a/gqcnn/analysis/analyzer.py +++ b/gqcnn/analysis/analyzer.py @@ -22,7 +22,7 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -Class for analyzing a GQCNN model for grasp quality prediction. +Class for analyzing a GQ-CNN model for grasp quality prediction. Author: Jeff Mahler """ from __future__ import absolute_import @@ -53,82 +53,84 @@ from ..grasping import Grasp2D, SuctionPoint2D from ..utils import GripperMode, ImageMode, GeneralConstants, read_pose_data, GQCNNFilenames -WINDOW = 100 +WINDOW = 100 # For loss. MAX_LOSS = 5.0 class GQCNNAnalyzer(object): - """ Analyzes a trained GQ-CNN model. """ + """Analyzes a trained GQ-CNN model.""" - def __init__(self, config, verbose=True, plot_backend='pdf'): + def __init__(self, config, verbose=True, plot_backend="pdf"): """ Parameters ---------- config : dict - dictionary of analysis configuration parameters + Dictionary of analysis configuration parameters. verbose : bool - whether or not to log analysis output to stdout + Whether or not to log analysis output to stdout. plot_backend : str - matplotlib plotting backend to use, default is non-interactive 'pdf' backend + Matplotlib plotting backend to use, default is non-interactive + "pdf" backend. """ self.cfg = config self.verbose = verbose - plt.switch_backend(plot_backend) # by default we want to use a non-interactive backend(ex. pdf) because the GQCNNAnalyzer anyways only saves plots to disk, and interactive backends sometimes cause issues with localhost when run remotely in screen + plt.switch_backend(plot_backend) # By default we want to use a non-interactive backend (ex. "pdf)" because the `GQCNNAnalyzer` anyways only saves plots to disk, and interactive backends sometimes cause issues with `localhost` when run remotely through the Linux `screen` program. self._parse_config() def _parse_config(self): - """ Read params from the config file """ - # plotting params - self.log_rate = self.cfg['log_rate'] - self.font_size = self.cfg['font_size'] - self.line_width = self.cfg['line_width'] - self.dpi = self.cfg['dpi'] - self.num_bins = self.cfg['num_bins'] - self.num_vis = self.cfg['num_vis'] + """Read params from the config file.""" + # Plotting params. + self.log_rate = self.cfg["log_rate"] + self.font_size = self.cfg["font_size"] + self.line_width = self.cfg["line_width"] + self.dpi = self.cfg["dpi"] + self.num_bins = self.cfg["num_bins"] + self.num_vis = self.cfg["num_vis"] def analyze(self, model_dir, output_dir, dataset_config=None): - """ Run analysis. + """Run analysis. Parameters ---------- model_dir : str - path to the GQ-CNN model to analyze + Path to the GQ-CNN model to analyze. output_dir : str - path to save the analysis + Path to save the analysis. dataset_config : dict - dictionary to configure dataset used for training evaluation if different from one used during training + Dictionary to configure dataset used for training evaluation if + different from one used during training. Returns ------- :obj:`autolab_core.BinaryClassificationResult` - result of analysis on training data + Result of analysis on training data. :obj:`autolab_core.BinaryClassificationResult` - result of analysis on validation data + Result of analysis on validation data. """ - # determine model output dir - model_name = '' + # Determine model output dir. + model_name = "" model_root = model_dir - while model_name == '' and model_root != '': + while model_name == "" and model_root != "": model_root, model_name = os.path.split(model_root) model_output_dir = os.path.join(output_dir, model_name) if not os.path.exists(model_output_dir): os.mkdir(model_output_dir) - # set up logger - self.logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(model_output_dir, 'analysis.log'), silence=(not self.verbose), global_log_file=self.verbose) + # Set up logger. + self.logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(model_output_dir, "analysis.log"), silence=(not self.verbose), global_log_file=self.verbose) - self.logger.info('Analyzing model %s' %(model_name)) - self.logger.info('Saving output to %s' %(output_dir)) + self.logger.info("Analyzing model %s" %(model_name)) + self.logger.info("Saving output to %s" %(output_dir)) - # run predictions + # Run predictions. train_result, val_result = self._run_prediction_single_model(model_dir, model_output_dir, dataset_config) - # finally plot curves + # Finally plot curves. init_train_error, final_train_error, init_train_loss, final_train_loss, init_val_error, final_val_error, norm_final_val_error = self._plot(model_dir, model_output_dir, train_result, @@ -137,7 +139,7 @@ def analyze(self, model_dir, return train_result, val_result, init_train_error, final_train_error, init_train_loss, final_train_loss, init_val_error, final_val_error, norm_final_val_error def _plot_grasp(self, datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=None): - """ Plots a single grasp represented as a datapoint. """ + """Plots a single grasp represented as a datapoint.""" image = DepthImage(datapoint[image_field_name][:,:,0]) depth = datapoint[pose_field_name][2] width = 0 @@ -164,19 +166,19 @@ def _plot_grasp(self, datapoint, image_field_name, pose_field_name, gripper_mode vis2d.imshow(image) for i, grasp in enumerate(grasps[:-1]): vis2d.grasp(grasp, width=width, color=plt.cm.RdYlGn(angular_preds[i * 2 + 1])) - vis2d.grasp(grasps[-1], width=width, color='b') + vis2d.grasp(grasps[-1], width=width, color="b") def _run_prediction_single_model(self, model_dir, model_output_dir, dataset_config): - """ Analyze the performance of a single model. """ - # read in model config - model_config_filename = os.path.join(model_dir, 'config.json') + """Analyze the performance of a single model.""" + # Read in model config. + model_config_filename = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) with open(model_config_filename) as data_file: model_config = json.load(data_file) - # load model - self.logger.info('Loading model %s' %(model_dir)) + # Load model. + self.logger.info("Loading model %s" %(model_dir)) log_file = None for handler in self.logger.handlers: if isinstance(handler, logging.FileHandler): @@ -186,28 +188,28 @@ def _run_prediction_single_model(self, model_dir, gripper_mode = gqcnn.gripper_mode angular_bins = gqcnn.angular_bins - # read params from the config + # Read params from the config. if dataset_config is None: - dataset_dir = model_config['dataset_dir'] - split_name = model_config['split_name'] - image_field_name = model_config['image_field_name'] - pose_field_name = model_config['pose_field_name'] - metric_name = model_config['target_metric_name'] - metric_thresh = model_config['metric_thresh'] + dataset_dir = model_config["dataset_dir"] + split_name = model_config["split_name"] + image_field_name = model_config["image_field_name"] + pose_field_name = model_config["pose_field_name"] + metric_name = model_config["target_metric_name"] + metric_thresh = model_config["metric_thresh"] else: - dataset_dir = dataset_config['dataset_dir'] - split_name = dataset_config['split_name'] - image_field_name = dataset_config['image_field_name'] - pose_field_name = dataset_config['pose_field_name'] - metric_name = dataset_config['target_metric_name'] - metric_thresh = dataset_config['metric_thresh'] - gripper_mode = dataset_config['gripper_mode'] + dataset_dir = dataset_config["dataset_dir"] + split_name = dataset_config["split_name"] + image_field_name = dataset_config["image_field_name"] + pose_field_name = dataset_config["pose_field_name"] + metric_name = dataset_config["target_metric_name"] + metric_thresh = dataset_config["metric_thresh"] + gripper_mode = dataset_config["gripper_mode"] - self.logger.info('Loading dataset %s' %(dataset_dir)) + self.logger.info("Loading dataset %s" %(dataset_dir)) dataset = TensorDataset.open(dataset_dir) train_indices, val_indices, _ = dataset.split(split_name) - # visualize conv filters + # Visualize conv filters. conv1_filters = gqcnn.filters num_filt = conv1_filters.shape[3] d = utils.sqrt_ceil(num_filt) @@ -216,20 +218,20 @@ def _run_prediction_single_model(self, model_dir, filt = conv1_filters[:,:,0,k] vis2d.subplot(d,d,k+1) vis2d.imshow(DepthImage(filt)) - figname = os.path.join(model_output_dir, 'conv1_filters.pdf') + figname = os.path.join(model_output_dir, "conv1_filters.pdf") vis2d.savefig(figname, dpi=self.dpi) - # aggregate training and validation true labels and predicted probabilities + # Aggregate training and validation true labels and predicted probabilities. all_predictions = [] if angular_bins > 0: all_predictions_raw = [] all_labels = [] for i in range(dataset.num_tensors): - # log progress + # Log progress. if i % self.log_rate == 0: - self.logger.info('Predicting tensor %d of %d' %(i+1, dataset.num_tensors)) + self.logger.info("Predicting tensor %d of %d" %(i+1, dataset.num_tensors)) - # read in data + # Read in data. image_arr = dataset.tensor(image_field_name, i).arr pose_arr = read_pose_data(dataset.tensor(pose_field_name, i).arr, gripper_mode) @@ -237,17 +239,18 @@ def _run_prediction_single_model(self, model_dir, label_arr = 1 * (metric_arr > metric_thresh) label_arr = label_arr.astype(np.uint8) if angular_bins > 0: - # form mask to extract predictions from ground-truth angular bins + # Form mask to extract predictions from ground-truth angular bins, raw_poses = dataset.tensor(pose_field_name, i).arr angles = raw_poses[:, 3] neg_ind = np.where(angles < 0) - angles = np.abs(angles) % GeneralConstants.PI + angles = np.abs(angles) % GeneralConstants.PI # TODO(vsatish): These should use the max angle instead. angles[neg_ind] *= -1 g_90 = np.where(angles > (GeneralConstants.PI / 2)) l_neg_90 = np.where(angles < (-1 * (GeneralConstants.PI / 2))) angles[g_90] -= GeneralConstants.PI angles[l_neg_90] += GeneralConstants.PI - angles *= -1 # hack to fix reverse angle convention + angles *= -1 # Hack to fix reverse angle convention. + # TODO(vsatish): Fix this along with the others. angles += (GeneralConstants.PI / 2) pred_mask = np.zeros((raw_poses.shape[0], angular_bins*2), dtype=bool) bin_width = GeneralConstants.PI / angular_bins @@ -255,22 +258,22 @@ def _run_prediction_single_model(self, model_dir, pred_mask[i, int((angles[i] // bin_width)*2)] = True pred_mask[i, int((angles[i] // bin_width)*2 + 1)] = True - # predict with GQ-CNN + # Predict with GQ-CNN. predictions = gqcnn.predict(image_arr, pose_arr) if angular_bins > 0: raw_predictions = np.array(predictions) predictions = predictions[pred_mask].reshape((-1, 2)) - # aggregate + # Aggregate. all_predictions.extend(predictions[:,1].tolist()) if angular_bins > 0: all_predictions_raw.extend(raw_predictions.tolist()) all_labels.extend(label_arr.tolist()) - # close session + # Close session. gqcnn.close_session() - # create arrays + # Create arrays. all_predictions = np.array(all_predictions) all_labels = np.array(all_labels) train_predictions = all_predictions[train_indices] @@ -282,32 +285,32 @@ def _run_prediction_single_model(self, model_dir, train_predictions_raw = all_predictions_raw[train_indices] val_predictions_raw = all_predictions_raw[val_indices] - # aggregate results + # Aggregate results. train_result = BinaryClassificationResult(train_predictions, train_labels) val_result = BinaryClassificationResult(val_predictions, val_labels) - train_result.save(os.path.join(model_output_dir, 'train_result.cres')) - val_result.save(os.path.join(model_output_dir, 'val_result.cres')) + train_result.save(os.path.join(model_output_dir, "train_result.cres")) + val_result.save(os.path.join(model_output_dir, "val_result.cres")) - # get stats, plot curves - self.logger.info('Model %s training error rate: %.3f' %(model_dir, train_result.error_rate)) - self.logger.info('Model %s validation error rate: %.3f' %(model_dir, val_result.error_rate)) + # Get stats, plot curves. + self.logger.info("Model %s training error rate: %.3f" %(model_dir, train_result.error_rate)) + self.logger.info("Model %s validation error rate: %.3f" %(model_dir, val_result.error_rate)) - self.logger.info('Model %s training loss: %.3f' %(model_dir, train_result.cross_entropy_loss)) - self.logger.info('Model %s validation loss: %.3f' %(model_dir, val_result.cross_entropy_loss)) + self.logger.info("Model %s training loss: %.3f" %(model_dir, train_result.cross_entropy_loss)) + self.logger.info("Model %s validation loss: %.3f" %(model_dir, val_result.cross_entropy_loss)) - # save images + # Save images. vis2d.figure() - example_dir = os.path.join(model_output_dir, 'examples') + example_dir = os.path.join(model_output_dir, "examples") if not os.path.exists(example_dir): os.mkdir(example_dir) - # train - self.logger.info('Saving training examples') - train_example_dir = os.path.join(example_dir, 'train') + # Train. + self.logger.info("Saving training examples") + train_example_dir = os.path.join(example_dir, "train") if not os.path.exists(train_example_dir): os.mkdir(train_example_dir) - # train TP + # Train TP. true_positive_indices = train_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] @@ -320,13 +323,13 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, 'true_positive_%03d.png' %(i))) + vis2d.savefig(os.path.join(train_example_dir, "true_positive_%03d.png" %(i))) - # train FP + # Train FP. false_positive_indices = train_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] @@ -339,13 +342,13 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, 'false_positive_%03d.png' %(i))) + vis2d.savefig(os.path.join(train_example_dir, "false_positive_%03d.png" %(i))) - # train TN + # Train TN. true_negative_indices = train_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] @@ -358,13 +361,13 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, 'true_negative_%03d.png' %(i))) + vis2d.savefig(os.path.join(train_example_dir, "true_negative_%03d.png" %(i))) - # train TP + # Train TP. false_negative_indices = train_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] @@ -377,19 +380,19 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, train_result.pred_probs[j], train_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, 'false_negative_%03d.png' %(i))) + vis2d.savefig(os.path.join(train_example_dir, "false_negative_%03d.png" %(i))) - # val - self.logger.info('Saving validation examples') - val_example_dir = os.path.join(example_dir, 'val') + # Val. + self.logger.info("Saving validation examples") + val_example_dir = os.path.join(example_dir, "val") if not os.path.exists(val_example_dir): os.mkdir(val_example_dir) - # val TP + # Val TP. true_positive_indices = val_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] @@ -402,13 +405,13 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, 'true_positive_%03d.png' %(i))) + vis2d.savefig(os.path.join(val_example_dir, "true_positive_%03d.png" %(i))) - # val FP + # Val FP. false_positive_indices = val_result.false_positive_indices np.random.shuffle(false_positive_indices) false_positive_indices = false_positive_indices[:self.num_vis] @@ -421,13 +424,13 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, 'false_positive_%03d.png' %(i))) + vis2d.savefig(os.path.join(val_example_dir, "false_positive_%03d.png" %(i))) - # val TN + # Val TN. true_negative_indices = val_result.true_negative_indices np.random.shuffle(true_negative_indices) true_negative_indices = true_negative_indices[:self.num_vis] @@ -440,13 +443,13 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, 'true_negative_%03d.png' %(i))) + vis2d.savefig(os.path.join(val_example_dir, "true_negative_%03d.png" %(i))) - # val TP + # Val TP. false_negative_indices = val_result.false_negative_indices np.random.shuffle(false_negative_indices) false_negative_indices = false_negative_indices[:self.num_vis] @@ -459,65 +462,65 @@ def _run_prediction_single_model(self, model_dir, self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) else: self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title('Datapoint %d: Pred: %.3f Label: %.3f' %(k, + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, 'false_negative_%03d.png' %(i))) + vis2d.savefig(os.path.join(val_example_dir, "false_negative_%03d.png" %(i))) - # save summary stats + # Save summary stats. train_summary_stats = { - 'error_rate': train_result.error_rate, - 'ap_score': train_result.ap_score, - 'auc_score': train_result.auc_score, - 'loss': train_result.cross_entropy_loss + "error_rate": train_result.error_rate, + "ap_score": train_result.ap_score, + "auc_score": train_result.auc_score, + "loss": train_result.cross_entropy_loss } - train_stats_filename = os.path.join(model_output_dir, 'train_stats.json') - json.dump(train_summary_stats, open(train_stats_filename, 'w'), + train_stats_filename = os.path.join(model_output_dir, "train_stats.json") + json.dump(train_summary_stats, open(train_stats_filename, "w"), indent=JSON_INDENT, sort_keys=True) val_summary_stats = { - 'error_rate': val_result.error_rate, - 'ap_score': val_result.ap_score, - 'auc_score': val_result.auc_score, - 'loss': val_result.cross_entropy_loss + "error_rate": val_result.error_rate, + "ap_score": val_result.ap_score, + "auc_score": val_result.auc_score, + "loss": val_result.cross_entropy_loss } - val_stats_filename = os.path.join(model_output_dir, 'val_stats.json') - json.dump(val_summary_stats, open(val_stats_filename, 'w'), + val_stats_filename = os.path.join(model_output_dir, "val_stats.json") + json.dump(val_summary_stats, open(val_stats_filename, "w"), indent=JSON_INDENT, sort_keys=True) return train_result, val_result def _plot(self, model_dir, model_output_dir, train_result, val_result): - """ Plot analysis curves """ - self.logger.info('Plotting') + """Plot analysis curves.""" + self.logger.info("Plotting") _, model_name = os.path.split(model_output_dir) - # set params - colors = ['g', 'b', 'c', 'y', 'm', 'r'] - styles = ['-', '--', '-.', ':', '-'] + # Set params. + colors = ["g", "b", "c", "y", "m", "r"] + styles = ["-", "--", "-.", ":", "-"] num_colors = len(colors) num_styles = len(styles) - # PR, ROC + # PR, ROC. vis2d.clf() train_result.precision_recall_curve(plot=True, line_width=self.line_width, color=colors[0], style=styles[0], - label='TRAIN') + label="TRAIN") val_result.precision_recall_curve(plot=True, line_width=self.line_width, color=colors[1], style=styles[1], - label='VAL') - vis2d.title('Precision Recall Curves', fontsize=self.font_size) + label="VAL") + vis2d.title("Precision Recall Curves", fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() - vis2d.legend(handles, labels, loc='best') - figname = os.path.join(model_output_dir, 'precision_recall.png') + vis2d.legend(handles, labels, loc="best") + figname = os.path.join(model_output_dir, "precision_recall.png") vis2d.savefig(figname, dpi=self.dpi) vis2d.clf() @@ -525,22 +528,22 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): line_width=self.line_width, color=colors[0], style=styles[0], - label='TRAIN') + label="TRAIN") val_result.roc_curve(plot=True, line_width=self.line_width, color=colors[1], style=styles[1], - label='VAL') - vis2d.title('Reciever Operating Characteristic', fontsize=self.font_size) + label="VAL") + vis2d.title("Reciever Operating Characteristic", fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() - vis2d.legend(handles, labels, loc='best') - figname = os.path.join(model_output_dir, 'roc.png') + vis2d.legend(handles, labels, loc="best") + figname = os.path.join(model_output_dir, "roc.png") vis2d.savefig(figname, dpi=self.dpi) - # plot histogram of prediction errors + # Plot histogram of prediction errors. num_bins = min(self.num_bins, train_result.num_datapoints) - # train positives + # Train positives. pos_ind = np.where(train_result.labels == 1)[0] diffs = np.abs(train_result.labels[pos_ind] - train_result.pred_probs[pos_ind]) vis2d.figure() @@ -549,13 +552,13 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): bounds=(0,1), normalized=False, plot=True) - vis2d.title('Error on Positive Training Examples', fontsize=self.font_size) - vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) - vis2d.ylabel('Count', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'pos_train_errors_histogram.png') + vis2d.title("Error on Positive Training Examples", fontsize=self.font_size) + vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) + vis2d.ylabel("Count", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "pos_train_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) - # train negatives + # Train negatives. neg_ind = np.where(train_result.labels == 0)[0] diffs = np.abs(train_result.labels[neg_ind] - train_result.pred_probs[neg_ind]) vis2d.figure() @@ -564,16 +567,16 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): bounds=(0,1), normalized=False, plot=True) - vis2d.title('Error on Negative Training Examples', fontsize=self.font_size) - vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) - vis2d.ylabel('Count', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'neg_train_errors_histogram.png') + vis2d.title("Error on Negative Training Examples", fontsize=self.font_size) + vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) + vis2d.ylabel("Count", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "neg_train_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) - # histogram of validation errors + # Histogram of validation errors. num_bins = min(self.num_bins, val_result.num_datapoints) - # val positives + # Val positives. pos_ind = np.where(val_result.labels == 1)[0] diffs = np.abs(val_result.labels[pos_ind] - val_result.pred_probs[pos_ind]) vis2d.figure() @@ -582,13 +585,13 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): bounds=(0,1), normalized=False, plot=True) - vis2d.title('Error on Positive Validation Examples', fontsize=self.font_size) - vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) - vis2d.ylabel('Count', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'pos_val_errors_histogram.png') + vis2d.title("Error on Positive Validation Examples", fontsize=self.font_size) + vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) + vis2d.ylabel("Count", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "pos_val_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) - # val negatives + # Val negatives. neg_ind = np.where(val_result.labels == 0)[0] diffs = np.abs(val_result.labels[neg_ind] - val_result.pred_probs[neg_ind]) vis2d.figure() @@ -597,13 +600,13 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): bounds=(0,1), normalized=False, plot=True) - vis2d.title('Error on Negative Validation Examples', fontsize=self.font_size) - vis2d.xlabel('Abs Prediction Error', fontsize=self.font_size) - vis2d.ylabel('Count', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'neg_val_errors_histogram.png') + vis2d.title("Error on Negative Validation Examples", fontsize=self.font_size) + vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) + vis2d.ylabel("Count", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "neg_val_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) - # losses + # Losses. try: train_errors_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_ERRORS) val_errors_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ERRORS) @@ -624,7 +627,7 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): val_errors = np.r_[pct_pos_val, val_errors] val_iters = np.r_[0, val_iters] - # window the training error + # Window the training error. i = 0 train_errors = [] train_losses = [] @@ -646,49 +649,49 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): norm_final_val_error = val_result.error_rate / pct_pos_val vis2d.clf() - vis2d.plot(train_iters, train_errors, linewidth=self.line_width, color='b') - vis2d.plot(val_iters, val_errors, linewidth=self.line_width, color='g') + vis2d.plot(train_iters, train_errors, linewidth=self.line_width, color="b") + vis2d.plot(val_iters, val_errors, linewidth=self.line_width, color="g") vis2d.ylim(0, 100) - vis2d.legend(('TRAIN (Minibatch)', 'VAL'), fontsize=self.font_size, loc='best') - vis2d.xlabel('Iteration', fontsize=self.font_size) - vis2d.ylabel('Error Rate', fontsize=self.font_size) - vis2d.title('Error Rate vs Training Iteration', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'training_error_rates.png') + vis2d.legend(("TRAIN (Minibatch)", "VAL"), fontsize=self.font_size, loc="best") + vis2d.xlabel("Iteration", fontsize=self.font_size) + vis2d.ylabel("Error Rate", fontsize=self.font_size) + vis2d.title("Error Rate vs Training Iteration", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "training_error_rates.png") vis2d.savefig(figname, dpi=self.dpi) vis2d.clf() - vis2d.plot(train_iters, norm_train_errors, linewidth=4, color='b') - vis2d.plot(val_iters, norm_val_errors, linewidth=4, color='g') + vis2d.plot(train_iters, norm_train_errors, linewidth=4, color="b") + vis2d.plot(val_iters, norm_val_errors, linewidth=4, color="g") vis2d.ylim(0, 2.0) - vis2d.legend(('TRAIN (Minibatch)', 'VAL'), fontsize=self.font_size, loc='best') - vis2d.xlabel('Iteration', fontsize=self.font_size) - vis2d.ylabel('Normalized Error Rate', fontsize=self.font_size) - vis2d.title('Normalized Error Rate vs Training Iteration', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'training_norm_error_rates.png') + vis2d.legend(("TRAIN (Minibatch)", "VAL"), fontsize=self.font_size, loc="best") + vis2d.xlabel("Iteration", fontsize=self.font_size) + vis2d.ylabel("Normalized Error Rate", fontsize=self.font_size) + vis2d.title("Normalized Error Rate vs Training Iteration", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "training_norm_error_rates.png") vis2d.savefig(figname, dpi=self.dpi) - train_losses[train_losses > MAX_LOSS] = MAX_LOSS # CAP LOSSES + train_losses[train_losses > MAX_LOSS] = MAX_LOSS # CAP LOSSES. vis2d.clf() - vis2d.plot(train_iters, train_losses, linewidth=self.line_width, color='b') + vis2d.plot(train_iters, train_losses, linewidth=self.line_width, color="b") vis2d.ylim(0, 2.0) - vis2d.xlabel('Iteration', fontsize=self.font_size) - vis2d.ylabel('Loss', fontsize=self.font_size) - vis2d.title('Training Loss vs Iteration', fontsize=self.font_size) - figname = os.path.join(model_output_dir, 'training_losses.png') + vis2d.xlabel("Iteration", fontsize=self.font_size) + vis2d.ylabel("Loss", fontsize=self.font_size) + vis2d.title("Training Loss vs Iteration", fontsize=self.font_size) + figname = os.path.join(model_output_dir, "training_losses.png") vis2d.savefig(figname, dpi=self.dpi) - # log - self.logger.info('TRAIN') - self.logger.info('Original error: %.3f' %(train_errors[0])) - self.logger.info('Final error: %.3f' %(train_result.error_rate)) - self.logger.info('Orig loss: %.3f' %(train_losses[0])) - self.logger.info('Final loss: %.3f' %(train_losses[-1])) + # Log. + self.logger.info("TRAIN") + self.logger.info("Original error: %.3f" %(train_errors[0])) + self.logger.info("Final error: %.3f" %(train_result.error_rate)) + self.logger.info("Orig loss: %.3f" %(train_losses[0])) + self.logger.info("Final loss: %.3f" %(train_losses[-1])) - self.logger.info('VAL') - self.logger.info('Original error: %.3f' %(pct_pos_val)) - self.logger.info('Final error: %.3f' %(val_result.error_rate)) - self.logger.info('Normalized error: %.3f' %(norm_final_val_error)) + self.logger.info("VAL") + self.logger.info("Original error: %.3f" %(pct_pos_val)) + self.logger.info("Final error: %.3f" %(val_result.error_rate)) + self.logger.info("Normalized error: %.3f" %(norm_final_val_error)) return train_errors[0], train_result.error_rate, train_losses[0], train_losses[-1], pct_pos_val, val_result.error_rate, norm_final_val_error except Exception as e: - self.logger.error('Failed to plot training curves!\n' + str(e)) + self.logger.error("Failed to plot training curves!\n" + str(e)) diff --git a/gqcnn/grasping/policy/__init__.py b/gqcnn/grasping/policy/__init__.py index 87fa48f8..17971154 100644 --- a/gqcnn/grasping/policy/__init__.py +++ b/gqcnn/grasping/policy/__init__.py @@ -26,10 +26,10 @@ from __future__ import division from __future__ import print_function -from fc_policy import (FullyConvolutionalGraspingPolicyParallelJaw, - FullyConvolutionalGraspingPolicySuction) -from policy import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, - PriorityCompositeGraspingPolicy, RgbdImageState, - GraspAction, UniformRandomGraspingPolicy) +from .fc_policy import (FullyConvolutionalGraspingPolicyParallelJaw, + FullyConvolutionalGraspingPolicySuction) +from .policy import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, + PriorityCompositeGraspingPolicy, RgbdImageState, + GraspAction, UniformRandomGraspingPolicy) __all__ = ["FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "RobustGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "UniformRandomGraspingPolicy", "RgbdImageState", "GraspAction"] diff --git a/gqcnn/grasping/policy/fc_policy.py b/gqcnn/grasping/policy/fc_policy.py index aa128198..d6f58b8b 100644 --- a/gqcnn/grasping/policy/fc_policy.py +++ b/gqcnn/grasping/policy/fc_policy.py @@ -33,6 +33,7 @@ import math import os +from future.utils import with_metaclass import matplotlib.pyplot as plt import numpy as np @@ -46,7 +47,7 @@ from .enums import SamplingMethod from .policy import GraspingPolicy, GraspAction -class FullyConvolutionalGraspingPolicy(with_metaclass(GraspingPolicy)): +class FullyConvolutionalGraspingPolicy(with_metaclass(ABCMeta, GraspingPolicy)): """Abstract grasp sampling policy class using Fully-Convolutional GQ-CNN network. """ @@ -312,14 +313,14 @@ def _sample_depths(self, raw_depth_im, raw_seg): def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): """Generate the actions to be returned.""" actions = [] - ang_bin_width = math.pi / preds.shape[-1] + ang_bin_width = GeneralConstants.PI / preds.shape[-1] # TODO(vsatish): These should use the max angle instead. for i in range(num_actions): im_idx = ind[i, 0] h_idx = ind[i, 1] w_idx = ind[i, 2] ang_idx = ind[i, 3] center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2])) - ang = math.pi / 2 - (ang_idx * ang_bin_width + ang_bin_width / 2) + ang = GeneralConstants.PI / 2 - (ang_idx * ang_bin_width + ang_bin_width / 2) depth = depths[im_idx, 0] grasp = Grasp2D(center, ang, depth, width=self._gripper_width, camera_intr=camera_intr) grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, ang_idx], DepthImage(images[im_idx])) diff --git a/gqcnn/grasping/policy/policy.py b/gqcnn/grasping/policy/policy.py index 6bec4ed1..05167190 100644 --- a/gqcnn/grasping/policy/policy.py +++ b/gqcnn/grasping/policy/policy.py @@ -48,7 +48,7 @@ RgbdImage, SegmentationImage, CameraIntrinsics) from visualization import Visualizer2D as vis -from .constraint_fn import GraspConstraintFnFactory +from ..constraint_fn import GraspConstraintFnFactory from ..grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D from ..grasp_quality_function import (GraspQualityFunctionFactory, GQCnnQualityFunction) @@ -220,7 +220,7 @@ def action(self, state): """Returns an action for a given state.""" pass -class GraspingPolicy(Policy): +class GraspingPolicy(with_metaclass(ABCMeta, Policy)): """Policy for robust grasping with Grasp Quality Convolutional Neural Networks (GQ-CNN). """ diff --git a/gqcnn/model/tf/fc_network_tf.py b/gqcnn/model/tf/fc_network_tf.py index f2693a9e..2bbbe350 100644 --- a/gqcnn/model/tf/fc_network_tf.py +++ b/gqcnn/model/tf/fc_network_tf.py @@ -37,7 +37,7 @@ import tensorflow as tf from .network_tf import GQCNNTF -from ...utils import TrainingMode, InputDepthMode +from ...utils import TrainingMode, InputDepthMode, GQCNNFilenames class FCGQCNNTF(GQCNNTF): """FC-GQ-CNN network implemented in Tensorflow. @@ -88,7 +88,7 @@ def load(model_dir, fc_config, log_file=None): :obj:`FCGQCNNTF` initialized FC-GQ-CNN """ - config_file = os.path.join(model_dir, "config.json") + config_file = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) with open(config_file) as data_file: train_config = json.load(data_file, object_pairs_hook=OrderedDict) @@ -96,7 +96,7 @@ def load(model_dir, fc_config, log_file=None): # Initialize weights and Tensorflow network. fcgqcnn = FCGQCNNTF(gqcnn_config, fc_config, log_file=log_file) - fcgqcnn.init_weights_file(os.path.join(model_dir, "model.ckpt")) + fcgqcnn.init_weights_file(os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL)) fcgqcnn.init_mean_and_std(model_dir) training_mode = train_config["training_mode"] if training_mode == TrainingMode.CLASSIFICATION: diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index db7dbdb7..87fe93c4 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -42,9 +42,9 @@ import tensorflow.contrib.framework as tcf from autolab_core import Logger -from gqcnn.utils import (reduce_shape, read_pose_data, pose_dim, - weight_name_to_layer_name, GripperMode, - TrainingMode, InputDepthMode) +from ...utils import (reduce_shape, read_pose_data, pose_dim, + weight_name_to_layer_name, GripperMode, + TrainingMode, InputDepthMode, GQCNNFilenames) class GQCNNWeights(object): """Helper struct for storing network weights.""" @@ -92,7 +92,7 @@ def load(model_dir, verbose=True, log_file=None): :obj:`GQCNNTF` initialized GQ-CNN """ - config_file = os.path.join(model_dir, "config.json") + config_file = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) with open(config_file) as data_file: train_config = json.load(data_file, object_pairs_hook=OrderedDict) @@ -183,7 +183,7 @@ def load(model_dir, verbose=True, log_file=None): # Initialize weights and Tensorflow network. gqcnn = GQCNNTF(gqcnn_config, verbose=verbose, log_file=log_file) - gqcnn.init_weights_file(os.path.join(model_dir, "model.ckpt")) + gqcnn.init_weights_file(os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL)) gqcnn.init_mean_and_std(model_dir) training_mode = train_config["training_mode"] if training_mode == TrainingMode.CLASSIFICATION: @@ -207,14 +207,14 @@ def init_mean_and_std(self, model_dir): # Load in means and stds. if self._input_depth_mode == InputDepthMode.POSE_STREAM: try: - self._im_mean = np.load(os.path.join(model_dir, "im_mean.npy")) - self._im_std = np.load(os.path.join(model_dir, "im_std.npy")) + self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames.IM_MEAN)) + self._im_std = np.load(os.path.join(model_dir, GQCNNFilenames.IM_STD)) except: # Support for legacy file naming convention. - self._im_mean = np.load(os.path.join(model_dir, "mean.npy")) - self._im_std = np.load(os.path.join(model_dir, "std.npy")) - self._pose_mean = np.load(os.path.join(model_dir, "pose_mean.npy")) - self._pose_std = np.load(os.path.join(model_dir, "pose_std.npy")) + self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames.LEG_MEAN)) + self._im_std = np.load(os.path.join(model_dir, GQCNNFilenames.LEG_STD)) + self._pose_mean = np.load(os.path.join(model_dir, GQCNNFilenames.POSE_MEAN)) + self._pose_std = np.load(os.path.join(model_dir, GQCNNFilenames.POSE_STD)) # Read the certain parts of the pose mean/std that we desire. if len(self._pose_mean.shape) > 0 and self._pose_mean.shape[0] != self._pose_dim: @@ -226,11 +226,11 @@ def init_mean_and_std(self, model_dir): self._pose_mean = read_pose_data(self._pose_mean, self._gripper_mode) self._pose_std = read_pose_data(self._pose_std, self._gripper_mode) elif self._input_depth_mode == InputDepthMode.SUB: - self._im_depth_sub_mean = np.load(os.path.join(model_dir, "im_depth_sub_mean.npy")) - self._im_depth_sub_std = np.load(os.path.join(model_dir, "im_depth_sub_std.npy")) + self._im_depth_sub_mean = np.load(os.path.join(model_dir, GQCNNFilenames.IM_DEPTH_SUB_MEAN)) + self._im_depth_sub_std = np.load(os.path.join(model_dir, GQCNNFilenames.IM_DEPTH_SUB_STD)) elif self._input_depth_mode == InputDepthMode.IM_ONLY: - self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames."im_mean.npy")) - self._im_std = np.load(os.path.join(model_dir, "im_std.npy")) + self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames.IM_MEAN)) + self._im_std = np.load(os.path.join(model_dir, GQCNNFilenames.IM_STD)) else: raise ValueError("Unsupported input depth mode: {}".format(self._input_depth_mode)) @@ -251,8 +251,8 @@ def set_base_network(self, model_dir): output_layer = base_model_config["output_layer"] # Read model. - ckpt_file = os.path.join(model_dir, "model.ckpt") - config_file = os.path.join(model_dir, "architecture.json") + ckpt_file = os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL) + config_file = os.path.join(model_dir, GQCNNFilenames.SAVED_ARCH) base_arch = json.load(open(config_file, "r"), object_pairs_hook=OrderedDict) # Read base layer names. diff --git a/gqcnn/search/resource_manager.py b/gqcnn/search/resource_manager.py index 188998c6..5fa3432c 100644 --- a/gqcnn/search/resource_manager.py +++ b/gqcnn/search/resource_manager.py @@ -128,7 +128,7 @@ def num_trials_to_schedule(self, num_pending_trials): total_gpu_mems, gpu_loads, gpu_mems = self._get_gpu_stats() max_possible_trials_gpu_load_per_device = [int((100 - gpu_load) // self._trial_gpu_load) for gpu_load in gpu_loads] max_possible_trials_gpu_mem_per_device = [int((total_gpu_mem - gpu_mem) // self._trial_gpu_mem) for total_gpu_mem, gpu_mem in zip(total_gpu_mems, gpu_mems)] - max_possible_trials_gpu_per_device = map(lambda (x, y): min(x, y), zip(max_possible_trials_gpu_load_per_device, max_possible_trials_gpu_mem_per_device)) + max_possible_trials_gpu_per_device = map(lambda x: min(x[0], x[1]), zip(max_possible_trials_gpu_load_per_device, max_possible_trials_gpu_mem_per_device)) self._logger.info("GPU loads: {}, GPU mems: {}, Max possible trials: {}".format("% ".join([str(gpu_load) for gpu_load in gpu_loads]) + "%", "MiB ".join([str(gpu_mem) for gpu_mem in gpu_mems]) + "MiB", sum(max_possible_trials_gpu_per_device))) num_trials_to_schedule = min(num_trials_to_schedule, sum(max_possible_trials_gpu_per_device)) diff --git a/gqcnn/search/search.py b/gqcnn/search/search.py index a6fef34b..f53a9242 100644 --- a/gqcnn/search/search.py +++ b/gqcnn/search/search.py @@ -35,7 +35,6 @@ import multiprocessing import operator import os -from Queue import Queue import sys import time @@ -47,7 +46,12 @@ from autolab_core import Logger -from ..utils import GQCNNTrainingStatus +from ..utils import is_py2, GQCNNTrainingStatus + +if is_py2(): + from Queue import Queue +else: + from queue import Queue class GQCNNSearch(object): def __init__(self, analysis_config, train_configs, datasets, split_names, base_models=[], output_dir=None, search_name=None, monitor_cpu=True, monitor_gpu=True, cpu_cores=[], gpu_devices=[]): diff --git a/gqcnn/search/utils.py b/gqcnn/search/utils.py index aa54a27d..bd30d1c3 100644 --- a/gqcnn/search/utils.py +++ b/gqcnn/search/utils.py @@ -92,7 +92,7 @@ def parse_master_train_config(train_config): num_params = [] for field in fields: num_params.append(len(get_nested_key(train_config, field))) - assert max(num_params) == min(num_params), "All fields in anchor tag "{}" do not have the same # of parameters to search over!".format(anchor_tag) + assert max(num_params) == min(num_params), "All fields in anchor tag \"{}\" do not have the same # of parameters to search over!".format(anchor_tag) # If there is nothing to search over just return the given config. if len(hyperparam_search_fields) == 0 and len(hyperparam_anchored_search_fields) == 0: diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index 8dbe530f..427e1376 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -43,7 +43,6 @@ import sys import threading import time -import Queue import cv2 import numpy as np @@ -59,7 +58,12 @@ from ...utils import (ImageMode, TrainingMode, GripperMode, InputDepthMode, GeneralConstants, TrainStatsLogger, pose_dim, read_pose_data, weight_name_to_layer_name, - GQCNNTrainingStatus, GQCNNFilenames) + is_py2, GQCNNTrainingStatus, GQCNNFilenames) + +if is_py2(): + import Queue +else: + import queue as Queue class GQCNNTrainerTF(object): """Trains a GQ-CNN with Tensorflow backend.""" @@ -432,8 +436,8 @@ def handler(signum, frame): # Save the model. if step % self.save_frequency == 0 and step > 0: - self.saver.save(self.sess, os.path.join(self.model_dir, "model_{}.ckpt".format(step))) - self.saver.save(self.sess, os.path.join(self.model_dir, "model.ckpt")) + self.saver.save(self.sess, os.path.join(self.model_dir, GQCNNFilenames.INTER_MODEL.format(step))) + self.saver.save(self.sess, os.path.join(self.model_dir, GQCNNFilenames.FINAL_MODEL)) # Launch tensorboard only after the first iteration. if not self.tensorboard_has_launched: @@ -455,7 +459,7 @@ def handler(signum, frame): # Log & save everything! self.train_stats_logger.log() - self.saver.save(self.sess, os.path.join(self.model_dir, "model.ckpt")) + self.saver.save(self.sess, os.path.join(self.model_dir, GQCNNFilenames.FINAL_MODEL)) except Exception as e: self._cleanup() @@ -809,7 +813,7 @@ def _save_configs(self): self.cfg["base_model_dir"] = self.base_model_dir # Save config. - out_config_filename = os.path.join(self.model_dir, "config.json") + out_config_filename = os.path.join(self.model_dir, GQCNNFilenames.SAVED_CFG) tempOrderedDict = collections.OrderedDict() for key in self.cfg: tempOrderedDict[key] = self.cfg[key] @@ -824,7 +828,7 @@ def _save_configs(self): shutil.copyfile(this_filename, out_train_filename) # Save architecture. - out_architecture_filename = os.path.join(self.model_dir, "architecture.json") + out_architecture_filename = os.path.join(self.model_dir, GQCNNFilenames.SAVED_ARCH) json.dump(self.cfg["gqcnn"]["architecture"], open(out_architecture_filename, "w"), indent=GeneralConstants.JSON_INDENT) diff --git a/gqcnn/utils/__init__.py b/gqcnn/utils/__init__.py index 989d9d25..37aaf47c 100644 --- a/gqcnn/utils/__init__.py +++ b/gqcnn/utils/__init__.py @@ -28,16 +28,17 @@ from .enums import (ImageMode, TrainingMode, GripperMode, InputDepthMode, GeneralConstants, - GQCNNTrainingStatus) + GQCNNTrainingStatus, GQCNNFilenames) from .policy_exceptions import (NoValidGraspsException, NoAntipodalPairsFoundException) from .train_stats_logger import TrainStatsLogger -from .utils import (set_cuda_visible_devices, pose_dim, - read_pose_data, reduce_shape, - weight_name_to_layer_name) +from .utils import (is_py2, set_cuda_visible_devices, pose_dim, + read_pose_data, reduce_shape, + weight_name_to_layer_name) -__all__ = ["set_cuda_visible_devices", "pose_dim", "read_pose_data", "reduce_shape", +__all__ = ["is_py2", "set_cuda_visible_devices", "pose_dim", "read_pose_data", "reduce_shape", "weight_name_to_layer_name", "ImageMode", "TrainingMode", "GripperMode", "InputDepthMode", "GeneralConstants", "GQCNNTrainingStatus", "NoValidGraspsException", "NoAntipodalPairsFoundException", - "TrainStatsLogger"] + "TrainStatsLogger", + "GQCNNFilenames"] diff --git a/gqcnn/utils/enums.py b/gqcnn/utils/enums.py index 25f1d252..30d7d74e 100644 --- a/gqcnn/utils/enums.py +++ b/gqcnn/utils/enums.py @@ -43,8 +43,8 @@ class GeneralConstants(object): NUM_PREFETCH_Q_WORKERS = 3 QUEUE_SLEEP = 0.001 PI = math.pi - FIGSIZE = 16 # For display. - + FIGSIZE = 16 # For visualization. + # Enum for image modalities. class ImageMode(object): BINARY = "binary" @@ -96,9 +96,17 @@ class GQCNNFilenames(object): VAL_LOSSES = "val_losses.npy" VAL_ERRORS = "val_errors.npy" + LEG_MEAN = "mean.npy" + LEG_STD = "std.npy" IM_MEAN = "im_mean.npy" IM_STD = "im_std.npy" IM_DEPTH_SUB_MEAN = "im_depth_sub_mean.npy" IM_DEPTH_SUB_STD = "im_depth_sub_std.npy" POSE_MEAN = "pose_mean.npy" POSE_STD = "pose_std.npy" + + FINAL_MODEL = "model.ckpt" + INTER_MODEL = "model_{}.ckpt" + + SAVED_ARCH = "architecture.json" + SAVED_CFG = "config.json" diff --git a/gqcnn/utils/utils.py b/gqcnn/utils/utils.py index af4ef073..3e1e562a 100644 --- a/gqcnn/utils/utils.py +++ b/gqcnn/utils/utils.py @@ -31,6 +31,7 @@ from functools import reduce import os +import sys import numpy as np @@ -40,6 +41,9 @@ # Set up logger. logger = Logger.get_logger("gqcnn/utils/utils.py") +def is_py2(): + return sys.version[0] == "2" + def set_cuda_visible_devices(gpu_list): """Sets CUDA_VISIBLE_DEVICES environment variable to only show certain gpus. @@ -133,7 +137,7 @@ def read_pose_data(pose_arr, gripper_mode): else: return pose_arr[:,2:4] else: - raise ValueError("Gripper mode %s not supported." %(gripper_mode)) + raise ValueError("Gripper mode \"{}\" not supported.".format(gripper_mode)) def reduce_shape(shape): """Get shape of a layer for flattening.""" From b0839382cfd853dc32c7b31b4835982918c97427 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sat, 25 May 2019 13:06:20 -0400 Subject: [PATCH 03/29] Finished first round of manual formatting. --- .style.yapf | 4 + ci/travis/format.sh | 30 +++ examples/antipodal_grasp_sampling.py | 81 +++--- examples/policy.py | 199 ++++++++------- examples/policy_ros.py | 94 +++---- examples/policy_with_image_proc.py | 134 +++++----- gqcnn/grasping/grasp_quality_function.py | 2 - gqcnn/training/tf/trainer_tf.py | 20 -- gqcnn/version.py | 2 +- ros_nodes/grasp_planner_node.py | 238 +++++++++--------- scripts/docker/build-docker.sh | 3 +- .../datasets/download_dex-net_2.0.sh | 2 +- .../datasets/download_dex-net_2.1.sh | 2 +- .../datasets/download_dex-net_3.0.sh | 2 +- .../datasets/download_dex-net_4.0_fc_pj.sh | 2 +- .../download_dex-net_4.0_fc_suction.sh | 2 +- .../datasets/download_dex-net_4.0_pj.sh | 2 +- .../datasets/download_dex-net_4.0_suction.sh | 2 +- .../datasets/download_example_datasets.sh | 8 +- scripts/downloads/download_example_data.sh | 11 +- scripts/downloads/models/download_models.sh | 8 +- .../policies/run_all_dex-net_2.0_examples.sh | 2 +- .../policies/run_all_dex-net_2.1_examples.sh | 2 +- .../policies/run_all_dex-net_3.0_examples.sh | 2 +- .../run_all_dex-net_4.0_fc_pj_examples.sh | 2 +- ...run_all_dex-net_4.0_fc_suction_examples.sh | 2 +- .../run_all_dex-net_4.0_pj_examples.sh | 2 +- .../run_all_dex-net_4.0_suction_examples.sh | 2 +- scripts/training/train_dex-net_2.0.sh | 2 +- scripts/training/train_dex-net_3.0.sh | 2 +- setup.py | 152 +++++------ tools/analyze_gqcnn_performance.py | 75 +++--- tools/finetune.py | 129 +++++----- tools/hyperparam_search.py | 72 ++++-- tools/plot_training_losses.py | 140 +++++------ tools/run_policy.py | 118 +++++---- tools/train.py | 112 +++++---- 37 files changed, 880 insertions(+), 784 deletions(-) create mode 100644 .style.yapf create mode 100644 ci/travis/format.sh diff --git a/.style.yapf b/.style.yapf new file mode 100644 index 00000000..b6a9c447 --- /dev/null +++ b/.style.yapf @@ -0,0 +1,4 @@ +[style] +based_on_style=pep8 +allow_split_before_dict_value=False +join_multiple_lines=False diff --git a/ci/travis/format.sh b/ci/travis/format.sh new file mode 100644 index 00000000..d07ada15 --- /dev/null +++ b/ci/travis/format.sh @@ -0,0 +1,30 @@ +#!/bin/bash + +# Script for YAPF formatting. Adapted from https://github.com/ray-project/ray/blob/master/ci/travis/format.sh. + +YAPF_FLAGS=( + '--style' "$ROOT/.style.yapf" + '--recursive' + '--parallel' +) + +YAPF_EXCLUDES=() + +# Format specified files +format() { + yapf --in-place "${YAPF_FLAGS[@]}" -- "$@" +} + +# Format all files, and print the diff to stdout for travis. +format_all() { + yapf --diff "${YAPF_FLAGS[@]}" "${YAPF_EXCLUDES[@]}" test python +} + +# This flag formats individual files. --files *must* be the first command line +# arg to use this option. +if [[ "$1" == '--files' ]]; then + format "${@:2}" + # If `--all` is passed, then any further arguments are ignored and the + # entire Python directory is formatted. +elif [[ "$1" == '--all' ]]; then + format_all diff --git a/examples/antipodal_grasp_sampling.py b/examples/antipodal_grasp_sampling.py index db6a8de9..66f53046 100644 --- a/examples/antipodal_grasp_sampling.py +++ b/examples/antipodal_grasp_sampling.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,12 +21,17 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Demonstrates image-based antipodal grasp candidate sampling, which is used in the -GQ-CNN-based grasping policy. Samples images from a BerkeleyAutomation/perception RgbdSensor. + +Demonstrates image-based antipodal grasp candidate sampling, which is used in +the Cross Entropy Method (CEM)-based GQ-CNN grasping policy. Samples images +from a BerkeleyAutomation/perception `RgbdSensor`. Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + + import argparse import os import sys @@ -33,52 +41,53 @@ from autolab_core import RigidTransform, YamlConfig, Logger from perception import RgbdImage, RgbdSensorFactory from visualization import Visualizer2D as vis + from gqcnn.grasping import AntipodalDepthImageGraspSampler -# set up logger -logger = Logger.get_logger('examples/antipodal_grasp_sampling.py') +# Set up logger. +logger = Logger.get_logger("examples/antipodal_grasp_sampling.py") -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Sample antipodal grasps on a depth image from an RgbdSensor') - parser.add_argument('--config_filename', type=str, default='cfg/examples/antipodal_grasp_sampling.yaml', help='path to configuration file to use') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Sample antipodal grasps on a depth image from an RgbdSensor") + parser.add_argument("--config_filename", type=str, default="cfg/examples/antipodal_grasp_sampling.yaml", help="path to configuration file to use") args = parser.parse_args() config_filename = args.config_filename - # read config + # Read config. config = YamlConfig(config_filename) - sensor_type = config['sensor']['type'] - sensor_frame = config['sensor']['frame'] - num_grasp_samples = config['num_grasp_samples'] - gripper_width = config['gripper_width'] - inpaint_rescale_factor = config['inpaint_rescale_factor'] - visualize_sampling = config['visualize_sampling'] - sample_config = config['sampling'] - - # read camera calib - tf_filename = '%s_to_world.tf' %(sensor_frame) - T_camera_world = RigidTransform.load(os.path.join(config['calib_dir'], sensor_frame, tf_filename)) - - # setup sensor - sensor = RgbdSensorFactory.sensor(sensor_type, config['sensor']) + sensor_type = config["sensor"]["type"] + sensor_frame = config["sensor"]["frame"] + num_grasp_samples = config["num_grasp_samples"] + gripper_width = config["gripper_width"] + inpaint_rescale_factor = config["inpaint_rescale_factor"] + visualize_sampling = config["visualize_sampling"] + sample_config = config["sampling"] + + # Read camera calib. + tf_filename = "%s_to_world.tf" %(sensor_frame) + T_camera_world = RigidTransform.load(os.path.join(config["calib_dir"], sensor_frame, tf_filename)) + + # Setup sensor. + sensor = RgbdSensorFactory.sensor(sensor_type, config["sensor"]) sensor.start() camera_intr = sensor.ir_intrinsics - # read images + # Read images. color_im, depth_im, _ = sensor.frames() color_im = color_im.inpaint(rescale_factor=inpaint_rescale_factor) depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) - # sample grasps + # Sample grasps. grasp_sampler = AntipodalDepthImageGraspSampler(sample_config, gripper_width) grasps = grasp_sampler.sample(rgbd_im, camera_intr, num_grasp_samples, segmask=None, seed=100, visualize=visualize_sampling) - # visualize + # Visualize. vis.figure() vis.imshow(depth_im) for grasp in grasps: vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True) - vis.title('Sampled grasps') + vis.title("Sampled grasps") vis.show() diff --git a/examples/policy.py b/examples/policy.py index ef96a5d2..b053ddce 100644 --- a/examples/policy.py +++ b/examples/policy.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,15 +21,17 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Displays robust grasps planned using a GQ-CNN-based policy on a set of saved RGB-D images. -The default configuration for the standard GQ-CNN policy is cfg/examples/policy.yaml. The default configuration for the Fully-Convolutional GQ-CNN policy is cfg/examples/fc_policy.yaml. -Author ------- -Jeff Mahler and Vishal Satish +Displays robust grasps planned using a GQ-CNN grapsing policy on a set of saved +RGB-D images. The default configuration for the standard GQ-CNN policy is +`cfg/examples/cfg/examples/gqcnn_pj.yaml`. The default configuration for the +Fully-Convolutional GQ-CNN policy is cfg/examples/fc_gqcnn_pj.yaml. +Author: Jeff Mahler, Vishal Satish """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import json import os @@ -41,19 +46,19 @@ from gqcnn.grasping import RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, RgbdImageState, FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction from gqcnn.utils import GripperMode, NoValidGraspsException -# set up logger -logger = Logger.get_logger('examples/policy.py') - -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Run a grasping policy on an example image') - parser.add_argument('model_name', type=str, default=None, help='name of a trained model to run') - parser.add_argument('--depth_image', type=str, default=None, help='path to a test depth image stored as a .npy file') - parser.add_argument('--segmask', type=str, default=None, help='path to an optional segmask to use') - parser.add_argument('--camera_intr', type=str, default=None, help='path to the camera intrinsics') - parser.add_argument('--model_dir', type=str, default=None, help='path to the folder in which the model is stored') - parser.add_argument('--config_filename', type=str, default=None, help='path to configuration file to use') - parser.add_argument('--fully_conv', action='store_true', help='run Fully-Convolutional GQ-CNN policy instead of standard GQ-CNN policy') +# Set up logger. +logger = Logger.get_logger("examples/policy.py") + +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Run a grasping policy on an example image") + parser.add_argument("model_name", type=str, default=None, help="name of a trained model to run") + parser.add_argument("--depth_image", type=str, default=None, help="path to a test depth image stored as a .npy file") + parser.add_argument("--segmask", type=str, default=None, help="path to an optional segmask to use") + parser.add_argument("--camera_intr", type=str, default=None, help="path to the camera intrinsics") + parser.add_argument("--model_dir", type=str, default=None, help="path to the folder in which the model is stored") + parser.add_argument("--config_filename", type=str, default=None, help="path to configuration file to use") + parser.add_argument("--fully_conv", action="store_true", help="run Fully-Convolutional GQ-CNN policy instead of standard GQ-CNN policy") args = parser.parse_args() model_name = args.model_name depth_im_filename = args.depth_image @@ -63,98 +68,98 @@ config_filename = args.config_filename fully_conv = args.fully_conv - assert not (fully_conv and depth_im_filename is not None and segmask_filename is None), 'Fully-Convolutional policy expects a segmask.' + assert not (fully_conv and depth_im_filename is not None and segmask_filename is None), "Fully-Convolutional policy expects a segmask." if depth_im_filename is None: if fully_conv: depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/examples/clutter/primesense/depth_0.npy') + "..", + "data/examples/clutter/primesense/depth_0.npy") else: depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/examples/single_object/primesense/depth_0.npy') + "..", + "data/examples/single_object/primesense/depth_0.npy") if fully_conv and segmask_filename is None: segmask_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/examples/clutter/primesense/segmask_0.png') + "..", + "data/examples/clutter/primesense/segmask_0.png") if camera_intr_filename is None: camera_intr_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/calib/primesense/primesense.intr') + "..", + "data/calib/primesense/primesense.intr") - # set model if provided + # Set model if provided. if model_dir is None: model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../models') + "../models") model_path = os.path.join(model_dir, model_name) - # get configs - model_config = json.load(open(os.path.join(model_path, 'config.json'), 'r')) + # Get configs. + model_config = json.load(open(os.path.join(model_path, "config.json"), "r")) try: - gqcnn_config = model_config['gqcnn'] - gripper_mode = gqcnn_config['gripper_mode'] + gqcnn_config = model_config["gqcnn"] + gripper_mode = gqcnn_config["gripper_mode"] except: - gqcnn_config = model_config['gqcnn_config'] - input_data_mode = gqcnn_config['input_data_mode'] - if input_data_mode == 'tf_image': + gqcnn_config = model_config["gqcnn_config"] + input_data_mode = gqcnn_config["input_data_mode"] + if input_data_mode == "tf_image": gripper_mode = GripperMode.LEGACY_PARALLEL_JAW - elif input_data_mode == 'tf_image_suction': + elif input_data_mode == "tf_image_suction": gripper_mode = GripperMode.LEGACY_SUCTION - elif input_data_mode == 'suction': + elif input_data_mode == "suction": gripper_mode = GripperMode.SUCTION - elif input_data_mode == 'multi_suction': + elif input_data_mode == "multi_suction": gripper_mode = GripperMode.MULTI_SUCTION - elif input_data_mode == 'parallel_jaw': + elif input_data_mode == "parallel_jaw": gripper_mode = GripperMode.PARALLEL_JAW else: - raise ValueError('Input data mode {} not supported!'.format(input_data_mode)) + raise ValueError("Input data mode {} not supported!".format(input_data_mode)) - # set config + # Set config. if config_filename is None: if gripper_mode == GripperMode.LEGACY_PARALLEL_JAW or gripper_mode == GripperMode.PARALLEL_JAW: if fully_conv: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/fc_gqcnn_pj.yaml') + "..", + "cfg/examples/fc_gqcnn_pj.yaml") else: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/gqcnn_pj.yaml') + "..", + "cfg/examples/gqcnn_pj.yaml") elif gripper_mode == GripperMode.LEGACY_SUCTION or gripper_mode == GripperMode.SUCTION: if fully_conv: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/fc_gqcnn_suction.yaml') + "..", + "cfg/examples/fc_gqcnn_suction.yaml") else: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/gqcnn_suction.yaml') + "..", + "cfg/examples/gqcnn_suction.yaml") - # read config + # Read config. config = YamlConfig(config_filename) - inpaint_rescale_factor = config['inpaint_rescale_factor'] - policy_config = config['policy'] - - # make relative paths absolute - if 'gqcnn_model' in policy_config['metric'].keys(): - policy_config['metric']['gqcnn_model'] = model_path - if not os.path.isabs(policy_config['metric']['gqcnn_model']): - policy_config['metric']['gqcnn_model'] = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - policy_config['metric']['gqcnn_model']) + inpaint_rescale_factor = config["inpaint_rescale_factor"] + policy_config = config["policy"] + + # Make relative paths absolute. + if "gqcnn_model" in policy_config["metric"]: + policy_config["metric"]["gqcnn_model"] = model_path + if not os.path.isabs(policy_config["metric"]["gqcnn_model"]): + policy_config["metric"]["gqcnn_model"] = os.path.join(os.path.dirname(os.path.realpath(__file__)), + "..", + policy_config["metric"]["gqcnn_model"]) - # setup sensor + # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) - # read images + # Read images. depth_data = np.load(depth_im_filename) depth_im = DepthImage(depth_data, frame=camera_intr.frame) color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), frame=camera_intr.frame) - # optionally read a segmask + # Optionally read a segmask. segmask = None if segmask_filename is not None: segmask = BinaryImage.open(segmask_filename) @@ -164,10 +169,10 @@ else: segmask = segmask.mask_binary(valid_px_mask) - # inpaint + # Inpaint. depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) - if 'input_images' in policy_config['vis'].keys() and policy_config['vis']['input_images']: + if "input_images" in policy_config["vis"] and policy_config["vis"]["input_images"]: vis.figure(size=(10,10)) num_plot = 1 if segmask is not None: @@ -179,46 +184,46 @@ vis.imshow(segmask) vis.show() - # create state + # Create state. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) - # set input sizes for fully-convolutional policy + # Set input sizes for fully-convolutional policy. if fully_conv: - policy_config['metric']['fully_conv_gqcnn_config']['im_height'] = depth_im.shape[0] - policy_config['metric']['fully_conv_gqcnn_config']['im_width'] = depth_im.shape[1] + policy_config["metric"]["fully_conv_gqcnn_config"]["im_height"] = depth_im.shape[0] + policy_config["metric"]["fully_conv_gqcnn_config"]["im_width"] = depth_im.shape[1] - # init policy + # Init policy. if fully_conv: - #TODO: @Vishal we should really be doing this in some factory policy - if policy_config['type'] == 'fully_conv_suction': + # TODO(vsatish): We should really be doing this in some factory policy. + if policy_config["type"] == "fully_conv_suction": policy = FullyConvolutionalGraspingPolicySuction(policy_config) - elif policy_config['type'] == 'fully_conv_pj': + elif policy_config["type"] == "fully_conv_pj": policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_config) else: - raise ValueError('Invalid fully-convolutional policy type: {}'.format(policy_config['type'])) + raise ValueError("Invalid fully-convolutional policy type: {}".format(policy_config["type"])) else: - policy_type = 'cem' - if 'type' in policy_config.keys(): - policy_type = policy_config['type'] - if policy_type == 'ranking': + policy_type = "cem" + if "type" in policy_config: + policy_type = policy_config["type"] + if policy_type == "ranking": policy = RobustGraspingPolicy(policy_config) - elif policy_type == 'cem': + elif policy_type == "cem": policy = CrossEntropyRobustGraspingPolicy(policy_config) else: - raise ValueError('Invalid policy type: {}'.format(policy_type)) + raise ValueError("Invalid policy type: {}".format(policy_type)) - # query policy + # Query policy. policy_start = time.time() action = policy(state) - logger.info('Planning took %.3f sec' %(time.time() - policy_start)) + logger.info("Planning took %.3f sec" %(time.time() - policy_start)) - # vis final grasp - if policy_config['vis']['final_grasp']: + # Vis final grasp. + if policy_config["vis"]["final_grasp"]: vis.figure(size=(10,10)) vis.imshow(rgbd_im.depth, - vmin=policy_config['vis']['vmin'], - vmax=policy_config['vis']['vmax']) + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) - vis.title('Planned grasp at depth {0:.3f}m with Q={1:.3f}'.format(action.grasp.depth, action.q_value)) + vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format(action.grasp.depth, action.q_value)) vis.show() diff --git a/examples/policy_ros.py b/examples/policy_ros.py index 13601b83..f4c7da3e 100644 --- a/examples/policy_ros.py +++ b/examples/policy_ros.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,18 +21,17 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Displays robust grasps planned using a GQ-CNN-based policy on a set of saved RGB-D images. The default configuration is cfg/examples/policy.yaml. - -Author ------- -Jeff Mahler +Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import logging -import IPython import numpy as np import os import rosgraph.roslogging as rl @@ -48,18 +50,18 @@ from gqcnn.msg import GQCNNGrasp, BoundingBox from gqcnn.srv import GQCNNGraspPlanner, GQCNNGraspPlannerBoundingBox, GQCNNGraspPlannerSegmask -if __name__ == '__main__': - # set up logger - logging.getLogger().setLevel(logging.INFO) +# Set up logger. +logger = Logger.get_logger("examples/policy_ros.py") - # parse args - parser = argparse.ArgumentParser(description='Run a grasping policy on an example image') - parser.add_argument('--depth_image', type=str, default=None, help='path to a test depth image stored as a .npy file') - parser.add_argument('--segmask', type=str, default=None, help='path to an optional segmask to use') - parser.add_argument('--camera_intr', type=str, default=None, help='path to the camera intrinsics') - parser.add_argument('--gripper_width', type=float, default=0.05, help='width of the gripper to plan for') - parser.add_argument('--namespace', type=str, default='gqcnn', help='namespace of the ROS grasp planning service') - parser.add_argument('--vis_grasp', type=bool, default=True, help='whether or not to visualize the grasp') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Run a grasping policy on an example image") + parser.add_argument("--depth_image", type=str, default=None, help="path to a test depth image stored as a .npy file") + parser.add_argument("--segmask", type=str, default=None, help="path to an optional segmask to use") + parser.add_argument("--camera_intr", type=str, default=None, help="path to the camera intrinsics") + parser.add_argument("--gripper_width", type=float, default=0.05, help="width of the gripper to plan for") + parser.add_argument("--namespace", type=str, default="gqcnn", help="namespace of the ROS grasp planning service") + parser.add_argument("--vis_grasp", type=bool, default=True, help="whether or not to visualize the grasp") args = parser.parse_args() depth_im_filename = args.depth_image segmask_filename = args.segmask @@ -68,36 +70,36 @@ namespace = args.namespace vis_grasp = args.vis_grasp - # initialize the ROS node - rospy.init_node('grasp_planning_example') + # Initialize the ROS node. + rospy.init_node("grasp_planning_example") logging.getLogger().addHandler(rl.RosStreamHandler()) - # setup filenames + # Setup filenames. if depth_im_filename is None: depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/examples/single_object/primesense/depth_0.npy') + "..", + "data/examples/single_object/primesense/depth_0.npy") if camera_intr_filename is None: camera_intr_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/calib/primesense/primesense.intr') + "..", + "data/calib/primesense/primesense.intr") - # wait for Grasp Planning Service and create Service Proxy - rospy.wait_for_service('%s/grasp_planner' %(namespace)) - rospy.wait_for_service('%s/grasp_planner_segmask' %(namespace)) - plan_grasp = rospy.ServiceProxy('%s/grasp_planner' %(namespace), GQCNNGraspPlanner) - plan_grasp_segmask = rospy.ServiceProxy('%s/grasp_planner_segmask' %(namespace), GQCNNGraspPlannerSegmask) + # Wait for grasp planning service and create service proxy. + rospy.wait_for_service("%s/grasp_planner" %(namespace)) + rospy.wait_for_service("%s/grasp_planner_segmask" %(namespace)) + plan_grasp = rospy.ServiceProxy("%s/grasp_planner" %(namespace), GQCNNGraspPlanner) + plan_grasp_segmask = rospy.ServiceProxy("%s/grasp_planner_segmask" %(namespace), GQCNNGraspPlannerSegmask) cv_bridge = CvBridge() - # setup sensor + # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) - # read images + # Read images. depth_im = DepthImage.open(depth_im_filename, frame=camera_intr.frame) color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), frame=camera_intr.frame) - # read segmask + # Read segmask. if segmask_filename is not None: segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame) grasp_resp = plan_grasp_segmask(color_im.rosmsg, @@ -110,7 +112,7 @@ camera_intr.rosmsg) grasp = grasp_resp.grasp - # convert to a grasp action + # Convert to a grasp action. grasp_type = grasp.grasp_type if grasp_type == GQCNNGrasp.PARALLEL_JAW: center = Point(np.array([grasp.center_px[0], grasp.center_px[1]]), @@ -128,21 +130,21 @@ grasp.depth, camera_intr=camera_intr) else: - raise ValueError('Grasp type %d not recognized!' %(grasp_type)) + raise ValueError("Grasp type %d not recognized!" %(grasp_type)) try: thumbnail = DepthImage(cv_bridge.imgmsg_to_cv2(grasp.thumbnail, desired_encoding="passthrough"), frame=camera_intr.frame) except CVBridgeError as e: - logging.error(e) - logging.error('Failed to convert image') + logger.error(e) + logger.error("Failed to convert image") sys.exit(1) action = GraspAction(grasp_2d, grasp.q_value, thumbnail) - # vis final grasp + # Vis final grasp. if vis_grasp: vis.figure(size=(10,10)) vis.imshow(depth_im, vmin=0.6, vmax=0.9) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) - vis.title('Planned grasp on depth (Q=%.3f)' %(action.q_value)) + vis.title("Planned grasp on depth (Q=%.3f)" %(action.q_value)) vis.show() diff --git a/examples/policy_with_image_proc.py b/examples/policy_with_image_proc.py index 0595d908..d730cd64 100644 --- a/examples/policy_with_image_proc.py +++ b/examples/policy_with_image_proc.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,44 +21,45 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Displays robust grasps planned using a GQ-CNN-based policy on a set of saved RGB-D images. The default configuration is cfg/examples/policy.yaml. - -Author ------- -Jeff Mahler +Author: Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import os import time +import numpy as np import pcl import skimage -import numpy as np from autolab_core import PointCloud, RigidTransform, YamlConfig, Logger from perception import BinaryImage, CameraIntrinsics, ColorImage, DepthImage, RgbdImage, SegmentationImage from visualization import Visualizer2D as vis + from gqcnn import RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, RgbdImageState CLUSTER_TOL = 0.0015 MIN_CLUSTER_SIZE = 100 MAX_CLUSTER_SIZE = 1000000 -# set up logger -logger = Logger.get_logger('tools/policy_with_image_proc.py') - -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Run a grasping policy on an example image') - parser.add_argument('--depth_image', type=str, default=None, help='path to a test depth image stored as a .npy file') - parser.add_argument('--segmask', type=str, default=None, help='path to an optional segmask to use') - parser.add_argument('--camera_intrinsics', type=str, default=None, help='path to the camera intrinsics') - parser.add_argument('--camera_pose', type=str, default=None, help='path to the camera pose') - parser.add_argument('--model_dir', type=str, default=None, help='path to a trained model to run') - parser.add_argument('--config_filename', type=str, default=None, help='path to configuration file to use') +# Set up logger. +logger = Logger.get_logger("tools/policy_with_image_proc.py") + +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Run a grasping policy on an example image") + parser.add_argument("--depth_image", type=str, default=None, help="path to a test depth image stored as a .npy file") + parser.add_argument("--segmask", type=str, default=None, help="path to an optional segmask to use") + parser.add_argument("--camera_intrinsics", type=str, default=None, help="path to the camera intrinsics") + parser.add_argument("--camera_pose", type=str, default=None, help="path to the camera pose") + parser.add_argument("--model_dir", type=str, default=None, help="path to a trained model to run") + parser.add_argument("--config_filename", type=str, default=None, help="path to configuration file to use") args = parser.parse_args() depth_im_filename = args.depth_image segmask_filename = args.segmask @@ -66,46 +70,46 @@ if depth_im_filename is None: depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/examples/single_object/depth_0.npy') + "..", + "data/examples/single_object/depth_0.npy") if camera_intr_filename is None: camera_intr_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/calib/primesense.intr') + "..", + "data/calib/primesense.intr") if camera_pose_filename is None: camera_pose_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'data/calib/primesense.tf') + "..", + "data/calib/primesense.tf") if config_filename is None: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/replication/dex-net_2.0.yaml') + "..", + "cfg/examples/replication/dex-net_2.0.yaml") - # read config + # Read config. config = YamlConfig(config_filename) - inpaint_rescale_factor = config['inpaint_rescale_factor'] - policy_config = config['policy'] + inpaint_rescale_factor = config["inpaint_rescale_factor"] + policy_config = config["policy"] - # make relative paths absolute + # Make relative paths absolute. if model_dir is not None: - policy_config['metric']['gqcnn_model'] = model_dir - if 'gqcnn_model' in policy_config['metric'].keys() and not os.path.isabs(policy_config['metric']['gqcnn_model']): - policy_config['metric']['gqcnn_model'] = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - policy_config['metric']['gqcnn_model']) + policy_config["metric"]["gqcnn_model"] = model_dir + if "gqcnn_model" in policy_config["metric"] and not os.path.isabs(policy_config["metric"]["gqcnn_model"]): + policy_config["metric"]["gqcnn_model"] = os.path.join(os.path.dirname(os.path.realpath(__file__)), + "..", + policy_config["metric"]["gqcnn_model"]) - # setup sensor + # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) T_camera_world = RigidTransform.load(camera_pose_filename) - # read images + # Read images. depth_data = np.load(depth_im_filename) depth_data = depth_data.astype(np.float32) / 1000.0 depth_im = DepthImage(depth_data, frame=camera_intr.frame) color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), frame=camera_intr.frame) - # optionally read a segmask + # Optionally read a segmask. mask = np.zeros( (camera_intr.height, camera_intr.width, 1), dtype=np.uint8) c = np.array([165, 460, 500, 135]) @@ -121,13 +125,13 @@ else: segmask = segmask.mask_binary(valid_px_mask) - # create new cloud + # Create new cloud. point_cloud = camera_intr.deproject(depth_im) point_cloud.remove_zero_points() pcl_cloud = pcl.PointCloud(point_cloud.data.T.astype(np.float32)) tree = pcl_cloud.make_kdtree() - # find large clusters (likely to be real objects instead of noise) + # Find large clusters (likely to be real objects instead of noise). ec = pcl_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(CLUSTER_TOL) ec.set_MinClusterSize(MIN_CLUSTER_SIZE) @@ -138,7 +142,7 @@ obj_segmask_data = np.zeros(depth_im.shape) - # read out all points in large clusters + # Read out all points in large clusters. cur_i = 0 for j, indices in enumerate(cluster_indices): num_points = len(indices) @@ -155,10 +159,10 @@ obj_segmask = SegmentationImage(obj_segmask_data.astype(np.uint8)) obj_segmask = obj_segmask.mask_binary(segmask) - # inpaint + # Inpaint. depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) - if 'input_images' in policy_config['vis'].keys() and policy_config['vis']['input_images']: + if "input_images" in policy_config["vis"] and policy_config["vis"]["input_images"]: vis.figure(size=(10,10)) num_plot = 3 vis.subplot(1,num_plot,1) @@ -177,34 +181,34 @@ vis3d.pose(T_camera_world.inverse()) vis3d.show() - # create state + # Create state. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) - # init policy - policy_type = 'cem' - if 'type' in policy_config.keys(): - policy_type = policy_config['type'] - if policy_type == 'ranking': + # Init policy. + policy_type = "cem" + if "type" in policy_config: + policy_type = policy_config["type"] + if policy_type == "ranking": policy = RobustGraspingPolicy(policy_config) else: policy = CrossEntropyRobustGraspingPolicy(policy_config) policy_start = time.time() action = policy(state) - logger.info('Planning took %.3f sec' %(time.time() - policy_start)) + logger.info("Planning took %.3f sec" %(time.time() - policy_start)) - # vis final grasp - if policy_config['vis']['final_grasp']: + # Vis final grasp. + if policy_config["vis"]["final_grasp"]: vis.figure(size=(10,10)) vis.imshow(rgbd_im.depth, - vmin=policy_config['vis']['vmin'], - vmax=policy_config['vis']['vmax']) + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) - vis.title('Planned grasp on depth (Q=%.3f)' %(action.q_value)) + vis.title("Planned grasp on depth (Q=%.3f)" %(action.q_value)) vis.show() - # get grasp pose + # Get grasp pose. T_grasp_camera = action.grasp.pose(grasp_approach_dir=-T_camera_world.inverse().z_axis) grasp_pose_msg = T_grasp_camera.pose_msg - # TODO: control to reach the grasp pose + # TODO: Control to reach the grasp pose. diff --git a/gqcnn/grasping/grasp_quality_function.py b/gqcnn/grasping/grasp_quality_function.py index 11b41f19..8beec0cd 100644 --- a/gqcnn/grasping/grasp_quality_function.py +++ b/gqcnn/grasping/grasp_quality_function.py @@ -881,7 +881,6 @@ def __del__(self): self._gqcnn.close_session() except: pass - del self @property def gqcnn(self): @@ -1174,7 +1173,6 @@ def __del__(self): self._fcgqcnn.close_session() except: pass - del self @property def gqcnn(self): diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index 427e1376..bae8f312 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -178,7 +178,6 @@ def _create_optimizer(self, loss, batch, var_list, learning_rate): gradients, variables = zip(*optimizer.compute_gradients(loss, var_list=var_list)) # Clip gradients to prevent exploding gradient problem. - gradients, global_grad_norm = tf.clip_by_global_norm(gradients, self.max_global_grad_norm) # Generate op to apply gradients. @@ -1076,12 +1075,6 @@ def _cleanup(self): # Close Tensorflow session. self.gqcnn.close_session() - # Free memory. - for layer_weights in self.weights.values(): - del layer_weights - del self.saver - del self.sess - def _flush_prefetch_queue(self): """Flush prefetch queue.""" self.logger.info("Flushing prefetch queue...") @@ -1257,10 +1250,6 @@ def _load_and_enqueue(self, seed): if self._angular_bins > 0: train_pred_mask[start_i:end_i] = train_pred_mask_arr.copy() - del train_images_arr - del train_poses_arr - del train_label_arr - # Update start index. start_i = end_i num_queued += num_loaded @@ -1276,11 +1265,6 @@ def _load_and_enqueue(self, seed): time.sleep(GeneralConstants.QUEUE_SLEEP) queue_stop = time.time() self.logger.debug("Queue batch took {} sec".format(round(queue_stop - queue_start, 3))) - del train_images - del train_poses - del train_labels - if self._angular_bins > 0: - del train_pred_mask def _distort(self, image_arr, pose_arr): """Adds noise to a batch of images.""" @@ -1403,10 +1387,6 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): all_predictions.extend(predictions[:,1].tolist()) all_labels.extend(labels.tolist()) - # Clean up. - del images - del poses - # Get learning result. result = None if self.training_mode == TrainingMode.CLASSIFICATION: diff --git a/gqcnn/version.py b/gqcnn/version.py index 4d3c61b0..5e44327d 100644 --- a/gqcnn/version.py +++ b/gqcnn/version.py @@ -26,4 +26,4 @@ from __future__ import division from __future__ import print_function -__version__ = "1.0.0" +__version__ = "1.1.0" diff --git a/ros_nodes/grasp_planner_node.py b/ros_nodes/grasp_planner_node.py index 37b3e85d..06cdfcfe 100755 --- a/ros_nodes/grasp_planner_node.py +++ b/ros_nodes/grasp_planner_node.py @@ -1,13 +1,16 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -19,25 +22,31 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +ROS Server for planning GQ-CNN grasps. + +Author +----- +Vishal Satish """ -""" -ROS Server for planning GQCNN grasps -Author: Vishal Satish -""" +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import json import math -import numpy as np import os -import rospy import time from cv_bridge import CvBridge, CvBridgeError +import numpy as np +import rospy from autolab_core import YamlConfig -from gqcnn.grasping import Grasp2D, SuctionPoint2D, CrossEntropyRobustGraspingPolicy, RgbdImageState -from gqcnn.utils import GripperMode, NoValidGraspsException from perception import CameraIntrinsics, ColorImage, DepthImage, BinaryImage, RgbdImage from visualization import Visualizer2D as vis +from gqcnn.grasping import Grasp2D, SuctionPoint2D, CrossEntropyRobustGraspingPolicy, RgbdImageState +from gqcnn.utils import GripperMode, NoValidGraspsException from geometry_msgs.msg import PoseStamped from std_msgs.msg import Header @@ -50,95 +59,95 @@ def __init__(self, cfg, cv_bridge, grasping_policy, grasp_pose_publisher): Parameters ---------- cfg : dict - dictionary of configuration parameters + Dictionary of configuration parameters. cv_bridge: :obj:`CvBridge` - ROS CvBridge + ROS `CvBridge`. grasping_policy: :obj:`GraspingPolicy` - grasping policy to use + Grasping policy to use. grasp_pose_publisher: :obj:`Publisher` - ROS Publisher to publish planned grasp's ROS Pose only for visualization + ROS publisher to publish pose of planned grasp for visualization. """ self.cfg = cfg self.cv_bridge = cv_bridge self.grasping_policy = grasping_policy self.grasp_pose_publisher = grasp_pose_publisher - # set min dimensions + # Set minimum input dimensions. self.pad = max( - math.ceil(np.sqrt(2) * (float(self.cfg['policy']['metric']['crop_width']) / 2)), - math.ceil(np.sqrt(2) * (float(self.cfg['policy']['metric']['crop_height']) / 2)) + math.ceil(np.sqrt(2) * (float(self.cfg["policy"]["metric"]["crop_width"]) / 2)), + math.ceil(np.sqrt(2) * (float(self.cfg["policy"]["metric"]["crop_height"]) / 2)) ) - self.min_width = 2 * self.pad + self.cfg['policy']['metric']['crop_width'] - self.min_height = 2 * self.pad + self.cfg['policy']['metric']['crop_height'] + self.min_width = 2 * self.pad + self.cfg["policy"]["metric"]["crop_width"] + self.min_height = 2 * self.pad + self.cfg["policy"]["metric"]["crop_height"] def read_images(self, req): - """ Reads images from a ROS service request + """Reads images from a ROS service request. Parameters --------- req: :obj:`ROS ServiceRequest` - ROS ServiceRequest for grasp planner service + ROS ServiceRequest for grasp planner service. """ - # get the raw depth and color images as ROS Image objects + # Get the raw depth and color images as ROS `Image` objects. raw_color = req.color_image raw_depth = req.depth_image - # get the raw camera info as ROS CameraInfo object + # Get the raw camera info as ROS `CameraInfo`. raw_camera_info = req.camera_info - # wrap the camera info in a perception CameraIntrinsics object + # Wrap the camera info in a BerkeleyAutomation/perception `CameraIntrinsics` object. camera_intr = CameraIntrinsics(raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) - # create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ### + # Create wrapped BerkeleyAutomation/perception RGB and depth images by unpacking the ROS images using ROS `CvBridge` try: color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2(raw_color, "rgb8"), frame=camera_intr.frame) depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2(raw_depth, desired_encoding = "passthrough"), frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) - # check image sizes + # Check image sizes. if color_im.height != depth_im.height or \ color_im.width != depth_im.width: - msg = 'Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d' %(color_im.height, color_im.width, depth_im.height, depth_im.width) + msg = "Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d" %(color_im.height, color_im.width, depth_im.height, depth_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) if color_im.height < self.min_height or color_im.width < self.min_width: - msg = 'Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d' %(self.min_height, self.min_width, color_im.height, color_im.width) + msg = "Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d" %(self.min_height, self.min_width, color_im.height, color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) return color_im, depth_im, camera_intr def plan_grasp(self, req): - """ Grasp planner request handler . + """Grasp planner request handler. Parameters --------- req: :obj:`ROS ServiceRequest` - ROS ServiceRequest for grasp planner service + ROS `ServiceRequest` for grasp planner service. """ color_im, depth_im, camera_intr = self.read_images(req) return self._plan_grasp(color_im, depth_im, camera_intr) def plan_grasp_bb(self, req): - """ Grasp planner request handler . + """Grasp planner request handler. Parameters --------- req: :obj:`ROS ServiceRequest` - ROS ServiceRequest for grasp planner service + `ROS ServiceRequest` for grasp planner service. """ color_im, depth_im, camera_intr = self.read_images(req) return self._plan_grasp(color_im, depth_im, camera_intr, bounding_box=req.bounding_box) def plan_grasp_segmask(self, req): - """ Grasp planner request handler . + """Grasp planner request handler. Parameters --------- req: :obj:`ROS ServiceRequest` - ROS ServiceRequest for grasp planner service + ROS `ServiceRequest` for grasp planner service. """ color_im, depth_im, camera_intr = self.read_images(req) raw_segmask = req.segmask @@ -148,7 +157,7 @@ def plan_grasp_segmask(self, req): rospy.logerr(cv_bridge_exception) if color_im.height != segmask.height or \ color_im.width != segmask.width: - msg = 'Images and segmask must be the same shape! Color image is %d x %d but segmask is %d x %d' %(color_im.height, color_im.width, segmask.height, segmask.width) + msg = "Images and segmask must be the same shape! Color image is %d x %d but segmask is %d x %d" %(color_im.height, color_im.width, segmask.height, segmask.width) rospy.logerr(msg) raise rospy.ServiceException(msg) @@ -159,47 +168,47 @@ def _plan_grasp(self, color_im, camera_intr, bounding_box=None, segmask=None): - """ Grasp planner request handler . + """Grasp planner request handler. Parameters --------- req: :obj:`ROS ServiceRequest` - ROS ServiceRequest for grasp planner service + ROS `ServiceRequest` for grasp planner service. """ - rospy.loginfo('Planning Grasp') + rospy.loginfo("Planning Grasp") - # inpaint images - color_im = color_im.inpaint(rescale_factor=self.cfg['inpaint_rescale_factor']) - depth_im = depth_im.inpaint(rescale_factor=self.cfg['inpaint_rescale_factor']) + # Inpaint images. + color_im = color_im.inpaint(rescale_factor=self.cfg["inpaint_rescale_factor"]) + depth_im = depth_im.inpaint(rescale_factor=self.cfg["inpaint_rescale_factor"]) - # init segmask + # Init segmask. if segmask is None: segmask = BinaryImage(255*np.ones(depth_im.shape).astype(np.uint8), frame=color_im.frame) - # visualize - if self.cfg['vis']['color_image']: + # Visualize. + if self.cfg["vis"]["color_image"]: vis.imshow(color_im) vis.show() - if self.cfg['vis']['depth_image']: + if self.cfg["vis"]["depth_image"]: vis.imshow(depth_im) vis.show() - if self.cfg['vis']['segmask'] and segmask is not None: + if self.cfg["vis"]["segmask"] and segmask is not None: vis.imshow(segmask) vis.show() - # aggregate color and depth images into a single perception rgbdimage + # Aggregate color and depth images into a single BerkeleyAutomation/perception `RgbdImage`. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) - # mask bounding box + # Mask bounding box. if bounding_box is not None: - # calc bb parameters + # Calc bb parameters. min_x = bounding_box.minX min_y = bounding_box.minY max_x = bounding_box.maxX max_y = bounding_box.maxY - # contain box to image->don't let it exceed image height/width bounds + # Contain box to image->don't let it exceed image height/width bounds. if min_x < 0: min_x = 0 if min_y < 0: @@ -209,14 +218,14 @@ def _plan_grasp(self, color_im, if max_y > rgbd_im.height: max_y = rgbd_im.height - # mask + # Mask. bb_segmask_arr = np.zeros([rgbd_image.height, rgbd_image.width]) bb_segmask_arr[min_y:max_y, min_x:max_x] = 255 bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8), segmask.frame) segmask = segmask.mask_binary(bb_segmask) - # visualize - if self.cfg['vis']['rgbd_state']: + # Visualize. + if self.cfg["vis"]["rgbd_state"]: masked_rgbd_im = rgbd_im.mask_binary(segmask) vis.figure() vis.subplot(1,2,1) @@ -225,37 +234,38 @@ def _plan_grasp(self, color_im, vis.imshow(masked_rgbd_im.depth) vis.show() - # create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics + # Create an `RgbdImageState` with the cropped `RgbdImage` and `CameraIntrinsics`. rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) - # execute policy + # Execute policy. try: return self.execute_policy(rgbd_state, self.grasping_policy, self.grasp_pose_publisher, camera_intr.frame) except NoValidGraspsException: - rospy.logerr('While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!') - raise rospy.ServiceException('While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!') + rospy.logerr("While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!") + raise rospy.ServiceException("While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!") def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher, pose_frame): - """ Executes a grasping policy on an RgbdImageState + """Executes a grasping policy on an `RgbdImageState`. Parameters ---------- rgbd_image_state: :obj:`RgbdImageState` - RgbdImageState from perception module to encapsulate depth and color image along with camera intrinsics + `RgbdImageState` from BerkeleyAutomation/perception to encapsulate + depth and color image along with camera intrinsics. grasping_policy: :obj:`GraspingPolicy` - grasping policy to use + Grasping policy to use. grasp_pose_publisher: :obj:`Publisher` - ROS Publisher to publish planned grasp's ROS Pose only for visualization + ROS publisher to publish pose of planned grasp for visualization. pose_frame: :obj:`str` - frame of reference to publish pose alone in + Frame of reference to publish pose in. """ - # execute the policy's action + # Execute the policy"s action. grasp_planning_start_time = time.time() grasp = grasping_policy(rgbd_image_state) - # create GQCNNGrasp return msg and populate it + # Create `GQCNNGrasp` return msg and populate it. gqcnn_grasp = GQCNNGrasp() gqcnn_grasp.q_value = grasp.q_value gqcnn_grasp.pose = grasp.grasp.pose().pose_msg @@ -264,17 +274,17 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher elif isinstance(grasp.grasp, SuctionPoint2D): gqcnn_grasp.grasp_type = GQCNNGrasp.SUCTION else: - rospy.logerr('Grasp type not supported!') - raise rospy.ServiceException('Grasp type not supported!') + rospy.logerr("Grasp type not supported!") + raise rospy.ServiceException("Grasp type not supported!") - # store grasp representation in image space + # Store grasp representation in image space. gqcnn_grasp.center_px[0] = grasp.grasp.center[0] gqcnn_grasp.center_px[1] = grasp.grasp.center[1] gqcnn_grasp.angle = grasp.grasp.angle gqcnn_grasp.depth = grasp.grasp.depth gqcnn_grasp.thumbnail = grasp.image.rosmsg - # create and publish the pose alone for visualization ease of grasp pose in rviz + # Create and publish the pose alone for easy visualization of grasp pose in Rviz. pose_stamped = PoseStamped() pose_stamped.pose = grasp.grasp.pose().pose_msg header = Header() @@ -283,75 +293,75 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher pose_stamped.header = header grasp_pose_publisher.publish(pose_stamped) - # return GQCNNGrasp msg - rospy.loginfo('Total grasp planning time: ' + str(time.time() - grasp_planning_start_time) + ' secs.') + # Return `GQCNNGrasp` msg. + rospy.loginfo("Total grasp planning time: " + str(time.time() - grasp_planning_start_time) + " secs.") return gqcnn_grasp -if __name__ == '__main__': +if __name__ == "__main__": - # initialize the ROS node - rospy.init_node('Grasp_Sampler_Server') + # Initialize the ROS node. + rospy.init_node("Grasp_Sampler_Server") - # initialize cv_bridge + # Initialize `CvBridge`. cv_bridge = CvBridge() - # get configs - model_name = rospy.get_param('~model_name') - model_dir = rospy.get_param('~model_dir') - if model_dir.lower() == 'default': + # Get configs. + model_name = rospy.get_param("~model_name") + model_dir = rospy.get_param("~model_dir") + if model_dir.lower() == "default": model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../models') + "../models") model_dir = os.path.join(model_dir, model_name) - model_config = json.load(open(os.path.join(model_dir, 'config.json'), 'r')) + model_config = json.load(open(os.path.join(model_dir, "config.json"), "r")) try: - gqcnn_config = model_config['gqcnn'] - gripper_mode = gqcnn_config['gripper_mode'] + gqcnn_config = model_config["gqcnn"] + gripper_mode = gqcnn_config["gripper_mode"] except: - gqcnn_config = model_config['gqcnn_config'] - input_data_mode = gqcnn_config['input_data_mode'] - if input_data_mode == 'tf_image': + gqcnn_config = model_config["gqcnn_config"] + input_data_mode = gqcnn_config["input_data_mode"] + if input_data_mode == "tf_image": gripper_mode = GripperMode.LEGACY_PARALLEL_JAW - elif input_data_mode == 'tf_image_suction': + elif input_data_mode == "tf_image_suction": gripper_mode = GripperMode.LEGACY_SUCTION - elif input_data_mode == 'suction': + elif input_data_mode == "suction": gripper_mode = GripperMode.SUCTION - elif input_data_mode == 'multi_suction': + elif input_data_mode == "multi_suction": gripper_mode = GripperMode.MULTI_SUCTION - elif input_data_mode == 'parallel_jaw': + elif input_data_mode == "parallel_jaw": gripper_mode = GripperMode.PARALLEL_JAW else: - raise ValueError('Input data mode {} not supported!'.format(input_data_mode)) + raise ValueError("Input data mode {} not supported!".format(input_data_mode)) - # set config + # Set config. config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/ros/gqcnn_suction.yaml') + "..", + "cfg/examples/ros/gqcnn_suction.yaml") if gripper_mode == GripperMode.LEGACY_PARALLEL_JAW or gripper_mode == GripperMode.PARALLEL_JAW: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/examples/ros/gqcnn_pj.yaml') + "..", + "cfg/examples/ros/gqcnn_pj.yaml") - # read config + # Read config. cfg = YamlConfig(config_filename) - policy_cfg = cfg['policy'] - policy_cfg['metric']['gqcnn_model'] = model_dir + policy_cfg = cfg["policy"] + policy_cfg["metric"]["gqcnn_model"] = model_dir - # create publisher to publish pose only of final grasp - grasp_pose_publisher = rospy.Publisher('/gqcnn_grasp/pose', PoseStamped, queue_size=10) + # Create publisher to publish pose of final grasp. + grasp_pose_publisher = rospy.Publisher("/gqcnn_grasp/pose", PoseStamped, queue_size=10) - # create a policy - rospy.loginfo('Creating Grasp Policy') + # Create a grasping policy. + rospy.loginfo("Creating Grasping Policy") grasping_policy = CrossEntropyRobustGraspingPolicy(policy_cfg) - # create a grasp planner object + # Create a grasp planner. grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher) - # initialize the service - grasp_planning_service = rospy.Service('grasp_planner', GQCNNGraspPlanner, grasp_planner.plan_grasp) - grasp_planning_service_bb = rospy.Service('grasp_planner_bounding_box', GQCNNGraspPlannerBoundingBox, grasp_planner.plan_grasp_bb) - grasp_planning_service_segmask = rospy.Service('grasp_planner_segmask', GQCNNGraspPlannerSegmask, grasp_planner.plan_grasp_segmask) - rospy.loginfo('Grasping Policy Initialized') + # Initialize the ROS service. + grasp_planning_service = rospy.Service("grasp_planner", GQCNNGraspPlanner, grasp_planner.plan_grasp) + grasp_planning_service_bb = rospy.Service("grasp_planner_bounding_box", GQCNNGraspPlannerBoundingBox, grasp_planner.plan_grasp_bb) + grasp_planning_service_segmask = rospy.Service("grasp_planner_segmask", GQCNNGraspPlannerSegmask, grasp_planner.plan_grasp_segmask) + rospy.loginfo("Grasping Policy Initialized") - # spin + # Spin forever. rospy.spin() diff --git a/scripts/docker/build-docker.sh b/scripts/docker/build-docker.sh index 19cbf6ec..ef29fa7a 100755 --- a/scripts/docker/build-docker.sh +++ b/scripts/docker/build-docker.sh @@ -1,4 +1,5 @@ -# build the CPU and GPU docker images +#!/bin/bash +# Build the CPU and GPU docker images. git archive --format=tar -o docker/gqcnn.tar --prefix=gqcnn/ master docker build --no-cache -t gqcnn/gpu -f docker/gpu/Dockerfile . diff --git a/scripts/downloads/datasets/download_dex-net_2.0.sh b/scripts/downloads/datasets/download_dex-net_2.0.sh index 1625465a..f01deaed 100755 --- a/scripts/downloads/datasets/download_dex-net_2.0.sh +++ b/scripts/downloads/datasets/download_dex-net_2.0.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash wget -O data/training/dexnet_2.zip https://berkeley.box.com/shared/static/15oid8m9q6n9cvr8og4vm37bwghjjslp.zip diff --git a/scripts/downloads/datasets/download_dex-net_2.1.sh b/scripts/downloads/datasets/download_dex-net_2.1.sh index bfa3bee8..a1453a99 100755 --- a/scripts/downloads/datasets/download_dex-net_2.1.sh +++ b/scripts/downloads/datasets/download_dex-net_2.1.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash wget -O data/training/dexnet_2.1.zip https://berkeley.box.com/shared/static/4g0g0lstl45hv5g5232f89aoeccjk32j.zip diff --git a/scripts/downloads/datasets/download_dex-net_3.0.sh b/scripts/downloads/datasets/download_dex-net_3.0.sh index 64fa254c..ed840c97 100755 --- a/scripts/downloads/datasets/download_dex-net_3.0.sh +++ b/scripts/downloads/datasets/download_dex-net_3.0.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash wget -O data/training/dexnet_3.tar.gz https://berkeley.box.com/shared/static/wd5s51f1n786i71t8dufckec0262za4f.gz diff --git a/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh b/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh index f5be91ab..f2e03856 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh @@ -1,3 +1,3 @@ -#!/bin/sh +#!/bin/bash echo "Dex-Net 4.0 FC-GQ-CNN PJ dataset not yet publicly available. Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for access." diff --git a/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh b/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh index f5be91ab..f2e03856 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh @@ -1,3 +1,3 @@ -#!/bin/sh +#!/bin/bash echo "Dex-Net 4.0 FC-GQ-CNN PJ dataset not yet publicly available. Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for access." diff --git a/scripts/downloads/datasets/download_dex-net_4.0_pj.sh b/scripts/downloads/datasets/download_dex-net_4.0_pj.sh index cbf55ad4..39558b2a 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_pj.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_pj.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash wget -O data/training/dexnet_4_pj_aa https://berkeley.box.com/shared/static/ybqo02q6471odc2k80pltjwj24dh1gkz.0_pj_aa wget -O data/training/dexnet_4_pj_ab https://berkeley.box.com/shared/static/3id22ohgprdiv02ne031dgue0oe1r264.0_pj_ab diff --git a/scripts/downloads/datasets/download_dex-net_4.0_suction.sh b/scripts/downloads/datasets/download_dex-net_4.0_suction.sh index 4afbd2f0..93ef8c20 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_suction.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_suction.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash wget -O data/training/dexnet_4_suction_aa https://berkeley.box.com/shared/static/ev8wc4xf6m1zf61wrud18qbr2y4f7wyn.0_suction_aa wget -O data/training/dexnet_4_suction_ab https://berkeley.box.com/shared/static/1dbkz11fnspxk8bg379lqo0931i4cmxx.0_suction_ab diff --git a/scripts/downloads/datasets/download_example_datasets.sh b/scripts/downloads/datasets/download_example_datasets.sh index 6ab76b1a..38688cf8 100755 --- a/scripts/downloads/datasets/download_example_datasets.sh +++ b/scripts/downloads/datasets/download_example_datasets.sh @@ -1,6 +1,6 @@ -#!/bin/sh +#!/bin/bash -# PARALLEL JAW +# PARALLEL JAW. wget -O data/training/example_training_dataset_pj.zip https://berkeley.box.com/shared/static/wpo8jbushrdq0adwjdsampui2tu1w1xz.zip mkdir -p data/training @@ -9,7 +9,7 @@ unzip example_training_dataset_pj.zip mv grasps example_pj cd ../.. -# SUCTION +# SUCTION. wget -O data/training/example_training_dataset_suction.zip https://berkeley.box.com/shared/static/fc9zb2cbql5rz6qtp11f6m7s0hyt1dwf.zip cd data/training @@ -17,7 +17,7 @@ unzip example_training_dataset_suction.zip mv grasps example_suction cd ../.. -# FULLY-CONVOLUTIONAL PARALLEL JAW +# FULLY-CONVOLUTIONAL PARALLEL JAW. wget -O data/training/example_training_dataset_pj_angular.zip https://berkeley.box.com/shared/static/2u4ew5444m90waucgsor8uoijgr9dgwr.zip cd data/training diff --git a/scripts/downloads/download_example_data.sh b/scripts/downloads/download_example_data.sh index 3b7cfa34..543d9555 100755 --- a/scripts/downloads/download_example_data.sh +++ b/scripts/downloads/download_example_data.sh @@ -1,6 +1,6 @@ -#!/bin/sh +#!/bin/bash -# DOWNLOAD MODELS (if they don't exist already) +# DOWNLOAD MODELS (if they don't exist already). mkdir -p models cd models @@ -20,9 +20,9 @@ fi cd .. -# DOWNLOAD DATASETS (if they don't already exist) +# DOWNLOAD DATASETS (if they don't already exist). -# PARALLEL JAW +# PARALLEL JAW. mkdir -p data/training cd data/training @@ -34,7 +34,7 @@ else echo "Found existing example PJ dataset..." fi -# SUCTION +# SUCTION. if [ ! -d "example_suction" ]; then wget -O example_training_dataset_suction.zip https://berkeley.box.com/shared/static/fc9zb2cbql5rz6qtp11f6m7s0hyt1dwf.zip unzip example_training_dataset_suction.zip @@ -44,4 +44,3 @@ else fi cd ../.. - diff --git a/scripts/downloads/models/download_models.sh b/scripts/downloads/models/download_models.sh index d701b0a8..dbc6934f 100755 --- a/scripts/downloads/models/download_models.sh +++ b/scripts/downloads/models/download_models.sh @@ -1,13 +1,15 @@ -#!/bin/sh +#!/bin/bash + mkdir -p models -# STANDARD + +# STANDARD. wget -O models/GQCNN-2.0.zip https://berkeley.box.com/shared/static/j4k4z6077ytucxpo6wk1c5hwj47mmpux.zip wget -O models/GQCNN-2.1.zip https://berkeley.box.com/shared/static/zr1gohe29r2dtaaq20iz0lqcbk5ub07y.zip wget -O models/GQCNN-3.0.zip https://berkeley.box.com/shared/static/8l47knzbzffu8zb9y5u46q0g0rvtuk74.zip wget -O models/GQCNN-4.0-PJ.zip https://berkeley.box.com/shared/static/boe4ilodi50hy5as5zun431s1bs7t97l.zip wget -O models/GQCNN-4.0-SUCTION.zip https://berkeley.box.com/shared/static/kzg19axnflhwys9t7n6bnuqsn18zj9wy.zip -# FULLY-CONVOLUTIONAL +# FULLY-CONVOLUTIONAL. wget -O models/FC-GQCNN-4.0-PJ.zip https://berkeley.box.com/shared/static/d9tvdnudd7f0743gxixcn0k0jeg1ds71.zip wget -O models/FC-GQCNN-4.0-SUCTION.zip https://berkeley.box.com/shared/static/ini7q54957u0cmaaxfihzn1i876m0ghd.zip diff --git a/scripts/policies/run_all_dex-net_2.0_examples.sh b/scripts/policies/run_all_dex-net_2.0_examples.sh index fb91c5bf..4e7d9aed 100755 --- a/scripts/policies/run_all_dex-net_2.0_examples.sh +++ b/scripts/policies/run_all_dex-net_2.0_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/policies/run_all_dex-net_2.1_examples.sh b/scripts/policies/run_all_dex-net_2.1_examples.sh index 4c4bb5f0..2f8b195d 100755 --- a/scripts/policies/run_all_dex-net_2.1_examples.sh +++ b/scripts/policies/run_all_dex-net_2.1_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/policies/run_all_dex-net_3.0_examples.sh b/scripts/policies/run_all_dex-net_3.0_examples.sh index 2a05b103..78f7f573 100755 --- a/scripts/policies/run_all_dex-net_3.0_examples.sh +++ b/scripts/policies/run_all_dex-net_3.0_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh b/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh index d5f2031f..f7802591 100755 --- a/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh b/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh index 775db926..068c8f62 100755 --- a/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/policies/run_all_dex-net_4.0_pj_examples.sh b/scripts/policies/run_all_dex-net_4.0_pj_examples.sh index 08d961e5..67d70e6d 100755 --- a/scripts/policies/run_all_dex-net_4.0_pj_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_pj_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/policies/run_all_dex-net_4.0_suction_examples.sh b/scripts/policies/run_all_dex-net_4.0_suction_examples.sh index 12833156..c66accfc 100755 --- a/scripts/policies/run_all_dex-net_4.0_suction_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_suction_examples.sh @@ -1,4 +1,4 @@ -#!/bin/sh +#!/bin/bash set -e diff --git a/scripts/training/train_dex-net_2.0.sh b/scripts/training/train_dex-net_2.0.sh index fa40dc59..10dcf496 100644 --- a/scripts/training/train_dex-net_2.0.sh +++ b/scripts/training/train_dex-net_2.0.sh @@ -1,3 +1,3 @@ -#!/bin/sh +#!/bin/bash python tools/train.py data/training/dex-net_2.0 --config_filename cfg/train_dex-net_2.0.yaml --name GQCNN-2.0 diff --git a/scripts/training/train_dex-net_3.0.sh b/scripts/training/train_dex-net_3.0.sh index 246ff48c..b12351f9 100644 --- a/scripts/training/train_dex-net_3.0.sh +++ b/scripts/training/train_dex-net_3.0.sh @@ -1,3 +1,3 @@ -#!/bin/sh +#!/bin/bash python tools/train.py data/training/dex-net_3.0 --config_filename cfg/train_dex-net_3.0.yaml --name GQCNN-3.0 diff --git a/setup.py b/setup.py index 1b206b41..c91b0974 100644 --- a/setup.py +++ b/setup.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,140 +21,139 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Setup of gqcnn python codebase. -Author ------- -Jeff Mahler & Vishal Satish +Setup of `gqcnn` Python codebase. +Author: Vishal Satish, Jeff Mahler """ -import os -import sys +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import logging -import subprocess +import os from setuptools import setup, find_packages from setuptools.command.develop import develop from setuptools.command.install import install +import subprocess +import sys -TF_MIN_VERSION = '1.10.0' -TF_MAX_VERSION = '1.13.1' +TF_MIN_VERSION = "1.10.0" +TF_MAX_VERSION = "1.13.1" -# set up logger -logging.basicConfig() # configure the root logger -logger = logging.getLogger('setup.py') +# Set up logger. +logging.basicConfig() # Configure the root logger. +logger = logging.getLogger("setup.py") logger.setLevel(logging.INFO) def get_tf_dep(): - # check whether or not the Nvidia driver and GPUs are available and add the corresponding Tensorflow dependency - tf_dep = 'tensorflow>={},<={}'.format(TF_MIN_VERSION, TF_MAX_VERSION) + # Check whether or not the Nvidia driver and GPUs are available and add the corresponding Tensorflow dependency. + tf_dep = "tensorflow>={},<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) try: - gpus = subprocess.check_output(['nvidia-smi', '--query-gpu=gpu_name', '--format=csv']).decode().strip().split('\n')[1:] + gpus = subprocess.check_output(["nvidia-smi", "--query-gpu=gpu_name", "--format=csv"]).decode().strip().split("\n")[1:] if len(gpus) > 0: - tf_dep = 'tensorflow-gpu>={},<={}'.format(TF_MIN_VERSION, TF_MAX_VERSION) + tf_dep = "tensorflow-gpu>={},<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) else: - logger.warning('Found Nvidia device driver but no devices...installing Tensorflow for CPU.') + logger.warning("Found Nvidia device driver but no devices...installing Tensorflow for CPU.") except OSError: - logger.warning('Could not find Nvidia device driver...installing Tensorflow for CPU.') + logger.warning("Could not find Nvidia device driver...installing Tensorflow for CPU.") return tf_dep -#TODO(vsatish): Use inheritance here +# TODO(vsatish): Use inheritance here. class DevelopCmd(develop): user_options_custom = [ - ('docker', None, 'installing in Docker'), + ("docker", None, "installing in Docker"), ] - user_options = getattr(develop, 'user_options', []) + user_options_custom + user_options = getattr(develop, "user_options", []) + user_options_custom def initialize_options(self): develop.initialize_options(self) - # initialize options + # Initialize options. self.docker = False def finalize_options(self): develop.finalize_options(self) def run(self): - # install Tensorflow dependency + # Install Tensorflow dependency. if not self.docker: tf_dep = get_tf_dep() - subprocess.Popen([sys.executable, '-m', 'pip', 'install', tf_dep]).wait() + subprocess.Popen([sys.executable, "-m", "pip", "install", tf_dep]).wait() else: - # if we're using docker, this will already have been installed explicitly through the correct {cpu/gpu}_requirements.txt; there is no way to check for CUDA/GPUs at docker build time because there is no easy way to set the nvidia runtime - logger.warning('Omitting Tensorflow dependency because of Docker installation.') #TODO(vsatish): Figure out why this isn't printed + # If we"re using Docker, this will already have been installed explicitly through the correct `{cpu/gpu}_requirements.txt`; there is no way to check for CUDA/GPUs at Docker build time because there is no easy way to set the Nvidia runtime. + logger.warning("Omitting Tensorflow dependency because of Docker installation.") # TODO(vsatish): Figure out why this isn"t printed. - # run installation + # Run installation. develop.run(self) class InstallCmd(install, object): user_options_custom = [ - ('docker', None, 'installing in Docker'), + ("docker", None, "installing in Docker"), ] - user_options = getattr(install, 'user_options', []) + user_options_custom + user_options = getattr(install, "user_options", []) + user_options_custom def initialize_options(self): install.initialize_options(self) - # initialize options + # Initialize options. self.docker = False def finalize_options(self): install.finalize_options(self) def run(self): - # install Tensorflow dependency + # Install Tensorflow dependency. if not self.docker: tf_dep = get_tf_dep() - subprocess.Popen([sys.executable, '-m', 'pip', 'install', tf_dep]).wait() + subprocess.Popen([sys.executable, "-m", "pip", "install", tf_dep]).wait() else: - # if we're using docker, this will already have been installed explicitly through the correct {cpu/gpu}_requirements.txt; there is no way to check for CUDA/GPUs at docker build time because there is no easy way to set the nvidia runtime - logger.warning('Omitting Tensorflow dependency because of Docker installation.') #TODO(vsatish): Figure out why this isn't printed + # If we"re using Docker, this will already have been installed explicitly through the correct `{cpu/gpu}_requirements.txt`; there is no way to check for CUDA/GPUs at Docker build time because there is no easy way to set the Nvidia runtime. + logger.warning("Omitting Tensorflow dependency because of Docker installation.") #TODO (vsatish): Figure out why this isn"t printed. - # run installation + # Run installation. install.run(self) requirements = [ - 'autolab-core', - 'autolab-perception', - 'visualization', - 'numpy', - 'scipy', - 'matplotlib', - 'opencv-python', - 'scikit-image', - 'scikit-learn', - 'psutil', - 'gputil' + "autolab-core", + "autolab-perception", + "visualization", + "numpy", + "scipy", + "matplotlib", + "opencv-python", + "scikit-image", + "scikit-learn", + "psutil", + "gputil" ] -exec(open(os.path.join(os.path.dirname(os.path.realpath(__file__)), 'gqcnn/version.py')).read()) +exec(open(os.path.join(os.path.dirname(os.path.realpath(__file__)), "gqcnn/version.py")).read()) -setup(name='gqcnn', +setup(name="gqcnn", version=__version__, - description='Project code for running Grasp Quality Convolutional Neural Networks', - author='Vishal Satish', - author_email='vsatish@berkeley.edu', - license = 'Berkeley Copyright', - url = 'https://github.com/BerkeleyAutomation/gqcnn', - keywords = 'robotics grasping vision deep learning', + description="Project code for running Grasp Quality Convolutional Neural Networks", + author="Vishal Satish", + author_email="vsatish@berkeley.edu", + license = "Berkeley Copyright", + url = "https://github.com/BerkeleyAutomation/gqcnn", + keywords = "robotics grasping vision deep learning", classifiers = [ - 'Development Status :: 4 - Beta', - 'Programming Language :: Python :: 2.7', - 'Programming Language :: Python :: 3', - 'Natural Language :: English', - 'Topic :: Scientific/Engineering' + "Development Status :: 4 - Beta", + "Programming Language :: Python :: 2.7", + "Programming Language :: Python :: 3", + "Natural Language :: English", + "Topic :: Scientific/Engineering" ], packages=find_packages(), install_requires = requirements, - extras_require = { 'docs' : [ - 'sphinx', - 'sphinxcontrib-napoleon', - 'sphinx_rtd_theme' + extras_require = { "docs" : [ + "sphinx", + "sphinxcontrib-napoleon", + "sphinx_rtd_theme" ], }, cmdclass={ - 'install': InstallCmd, - 'develop': DevelopCmd + "install": InstallCmd, + "develop": DevelopCmd } ) - diff --git a/tools/analyze_gqcnn_performance.py b/tools/analyze_gqcnn_performance.py index 4caff8c5..021fb43b 100644 --- a/tools/analyze_gqcnn_performance.py +++ b/tools/analyze_gqcnn_performance.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,34 +21,36 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Analyzes a GQ-CNN model. Author ------ Vishal Satish and Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import os -import time -import os import sys +import time from autolab_core import YamlConfig, Logger from gqcnn import GQCNNAnalyzer -# setup logger -logger = Logger.get_logger('tools/analyze_gqcnn_performance.py') +# Setup logger. +logger = Logger.get_logger("tools/analyze_gqcnn_performance.py") -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Analyze a Grasp Quality Convolutional Neural Network with TensorFlow') - parser.add_argument('model_name', type=str, default=None, help='name of model to analyze') - parser.add_argument('--output_dir', type=str, default=None, help='path to save the analysis') - parser.add_argument('--dataset_config_filename', type=str, default=None, help='path to a configuration file for testing on a custom dataset') - parser.add_argument('--config_filename', type=str, default=None, help='path to the configuration file to use') - parser.add_argument('--model_dir', type=str, default=None, help='path to the model') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Analyze a Grasp Quality Convolutional Neural Network with TensorFlow") + parser.add_argument("model_name", type=str, default=None, help="name of model to analyze") + parser.add_argument("--output_dir", type=str, default=None, help="path to save the analysis") + parser.add_argument("--dataset_config_filename", type=str, default=None, help="path to a configuration file for testing on a custom dataset") + parser.add_argument("--config_filename", type=str, default=None, help="path to the configuration file to use") + parser.add_argument("--model_dir", type=str, default=None, help="path to the model") args = parser.parse_args() model_name = args.model_name output_dir = args.output_dir @@ -53,29 +58,29 @@ config_filename = args.config_filename model_dir = args.model_dir - # create model dir + # Create model dir. if model_dir is None: model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../models') + "../models") model_dir = os.path.join(model_dir, model_name) - # if model_dir contains many models, analyze each of them + # If `model_dir` contains many models, analyze all of them. model_dir = [model_dir] - if 'config.json' not in os.listdir(model_dir[0]): - logger.warning('Found multiple models in model_dir, analyzing all of them...') + if "config.json" not in os.listdir(model_dir[0]): + logger.warning("Found multiple models in model_dir, analyzing all of them...") models = os.listdir(model_dir[0]) model_dir = [os.path.join(model_dir[0], model) for model in models] - # set defaults + # Set defaults. if output_dir is None: output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../analysis') + "../analysis") if config_filename is None: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/tools/analyze_gqcnn_performance.yaml') + "..", + "cfg/tools/analyze_gqcnn_performance.yaml") - # turn relative paths absolute + # Turn relative paths absolute. if not os.path.isabs(output_dir): output_dir = os.path.join(os.getcwd(), output_dir) if not os.path.isabs(config_filename): @@ -83,18 +88,18 @@ if dataset_config_filename is not None and not os.path.isabs(dataset_config_filename): config_filename = os.path.join(os.getcwd(), dataset_config_filename) - # make the output dir + # Make the output dir. if not os.path.exists(output_dir): os.mkdir(output_dir) - # read config + # Read config. config = YamlConfig(config_filename) dataset_config = None if dataset_config_filename is not None: dataset_config = YamlConfig(dataset_config_filename) - # run the analyzer - analyzer = GQCNNAnalyzer(config, plot_backend='pdf') + # Run the analyzer. + analyzer = GQCNNAnalyzer(config, plot_backend="pdf") for model in model_dir: analyzer.analyze(model, output_dir, dataset_config) diff --git a/tools/finetune.py b/tools/finetune.py index 4199333b..098d72f0 100644 --- a/tools/finetune.py +++ b/tools/finetune.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,53 +21,55 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Script for training a Grasp Quality Neural Network (GQ-CNN) from scratch. + +Script for fine-tuning a Grasp Quality Convolutional Neural Network (GQ-CNN). Author ------ Vishal Satish & Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import os -import time -import os import sys +import time -import autolab_core.utils as utils from autolab_core import YamlConfig, Logger +import autolab_core.utils as utils from gqcnn import get_gqcnn_model, get_gqcnn_trainer from gqcnn import utils as gqcnn_utils -# setup logger -logger = Logger.get_logger('tools/finetune.py') +# Setup logger. +logger = Logger.get_logger("tools/finetune.py") -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Fine-Tune a pre-trained Grasp Quality Convolutional Neural Network with TensorFlow') - parser.add_argument('dataset_dir', type=str, default=None, - help='path to the dataset to use for training and validation') - parser.add_argument('base_model_name', type=str, default=None, - help='name of the pre-trained model to fine-tune') - parser.add_argument('--split_name', type=str, default='image_wise', - help='name of the split to train on') - parser.add_argument('--output_dir', type=str, default=None, - help='path to store the model') - parser.add_argument('--tensorboard_port', type=int, default=None, - help='port to launch tensorboard on') - parser.add_argument('--seed', type=int, default=None, - help='random seed for training') - parser.add_argument('--config_filename', type=str, default=None, - help='path to the configuration file to use') - parser.add_argument('--model_dir', type=str, default=None, - help='path to the pre-trained model to fine-tune') - parser.add_argument('--name', type=str, default=None, - help='name for the trained model') - parser.add_argument('--save_datetime', type=bool, default=False, - help='whether or not to save a model with the date and time of training') - parser.add_argument('--backend', type=str, default='tf', - help='the deep learning framework to use') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Fine-Tune a pre-trained Grasp Quality Convolutional Neural Network with TensorFlow") + parser.add_argument("dataset_dir", type=str, default=None, + help="path to the dataset to use for training and validation") + parser.add_argument("base_model_name", type=str, default=None, + help="name of the pre-trained model to fine-tune") + parser.add_argument("--split_name", type=str, default="image_wise", + help="name of the split to train on") + parser.add_argument("--output_dir", type=str, default=None, + help="path to store the model") + parser.add_argument("--tensorboard_port", type=int, default=None, + help="port to launch tensorboard on") + parser.add_argument("--seed", type=int, default=None, + help="random seed for training") + parser.add_argument("--config_filename", type=str, default=None, + help="path to the configuration file to use") + parser.add_argument("--model_dir", type=str, default=None, + help="path to the pre-trained model to fine-tune") + parser.add_argument("--name", type=str, default=None, + help="name for the trained model") + parser.add_argument("--save_datetime", type=bool, default=False, + help="whether or not to save a model with the date and time of training") + parser.add_argument("--backend", type=str, default="tf", + help="the deep learning framework to use") args = parser.parse_args() dataset_dir = args.dataset_dir base_model_name = args.base_model_name @@ -78,23 +83,23 @@ save_datetime = args.save_datetime backend = args.backend - # set default output dir + # Set default output dir. if output_dir is None: output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../models') + "../models") - # set default config filename + # Set default config filename. if config_filename is None: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/finetune.yaml') + "..", + "cfg/finetune.yaml") - # set default model dir + # Set default model dir. if model_dir is None: model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../models') + "../models") - # turn relative paths absolute + # Turn relative paths absolute. if not os.path.isabs(dataset_dir): dataset_dir = os.path.join(os.getcwd(), dataset_dir) if not os.path.isabs(model_dir): @@ -104,33 +109,33 @@ if not os.path.isabs(config_filename): config_filename = os.path.join(os.getcwd(), config_filename) - # create full path to the pre-trained model + # Create full path to the pre-trained model. model_dir = os.path.join(model_dir, base_model_name) - # create output dir if necessary + # Create output dir if necessary. utils.mkdir_safe(output_dir) - # open train config + # Open train config. train_config = YamlConfig(config_filename) if seed is not None: - train_config['seed'] = seed - train_config['gqcnn']['seed'] = seed + train_config["seed"] = seed + train_config["gqcnn"]["seed"] = seed if tensorboard_port is not None: - train_config['tensorboard_port'] = tensorboard_port - gqcnn_params = train_config['gqcnn'] + train_config["tensorboard_port"] = tensorboard_port + gqcnn_params = train_config["gqcnn"] - # create a unique output folder based on the date and time + # Create a unique output folder based on the date and time. if save_datetime: - # create output dir + # Create output dir. unique_name = time.strftime("%Y%m%d-%H%M%S") output_dir = os.path.join(output_dir, unique_name) utils.mkdir_safe(output_dir) - # set visible devices - if 'gpu_list' in train_config: - gqcnn_utils.set_cuda_visible_devices(train_config['gpu_list']) + # Set visible devices. + if "gpu_list" in train_config: + gqcnn_utils.set_cuda_visible_devices(train_config["gpu_list"]) - # fine-tune the network + # Fine-tune the network. start_time = time.time() gqcnn = get_gqcnn_model(backend)(gqcnn_params) trainer = get_gqcnn_trainer(backend)(gqcnn, @@ -140,4 +145,4 @@ train_config, name=name) trainer.finetune(model_dir) - logger.info('Total Fine-tuning Time: ' + str(utils.get_elapsed_time(time.time() - start_time))) + logger.info("Total Fine-tuning Time: " + str(utils.get_elapsed_time(time.time() - start_time))) diff --git a/tools/hyperparam_search.py b/tools/hyperparam_search.py index 2fb69cb5..087b6562 100644 --- a/tools/hyperparam_search.py +++ b/tools/hyperparam_search.py @@ -1,31 +1,59 @@ +# -*- coding: utf-8 -*- """ -Script for searching over Grasp Quality Convolutional Neural Network (GQ-CNN) hyper-parameters. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Script for searching over Grasp Quality Convolutional Neural Network (GQ-CNN) +hyper-parameters. Author ------ Vishal Satish """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import sys -from gqcnn import GQCNNSearch from autolab_core import YamlConfig, Logger +from gqcnn import GQCNNSearch + +# Set up logger. +logger = Logger.get_logger("tools/hyperparam_search.py") -# set up logger -logger = Logger.get_logger('tools/hyperparam_search.py') - -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Hyper-parameter search for GQ-CNN.') - parser.add_argument('datasets', nargs='+', default=None, help='path to datasets') - parser.add_argument('--base_model_dirs', nargs='+', default=[], help='path to pre-trained base models for fine-tuning') - parser.add_argument('--train_configs', nargs='+', default=['cfg/train.yaml'], help='path to training configs') - parser.add_argument('--analysis_config', type=str, default='cfg/tools/analyze_gqcnn_performance.yaml') - parser.add_argument('--split_names', nargs='+', default=['image_wise'], help='dataset splits to use') - parser.add_argument('--output_dir', type=str, default='models', help='path to store search data') - parser.add_argument('--search_name', type=str, default=None, help='name of search') - parser.add_argument('--cpu_cores', nargs='+', default=[], help='CPU cores to use') - parser.add_argument('--gpu_devices', nargs='+', default=[], help='GPU devices to use') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Hyper-parameter search for GQ-CNN.") + parser.add_argument("datasets", nargs="+", default=None, help="path to datasets") + parser.add_argument("--base_model_dirs", nargs="+", default=[], help="path to pre-trained base models for fine-tuning") + parser.add_argument("--train_configs", nargs="+", default=["cfg/train.yaml"], help="path to training configs") + parser.add_argument("--analysis_config", type=str, default="cfg/tools/analyze_gqcnn_performance.yaml") + parser.add_argument("--split_names", nargs="+", default=["image_wise"], help="dataset splits to use") + parser.add_argument("--output_dir", type=str, default="models", help="path to store search data") + parser.add_argument("--search_name", type=str, default=None, help="name of search") + parser.add_argument("--cpu_cores", nargs="+", default=[], help="CPU cores to use") + parser.add_argument("--gpu_devices", nargs="+", default=[], help="GPU devices to use") args = parser.parse_args() datasets = args.datasets base_model_dirs = args.base_model_dirs @@ -37,20 +65,20 @@ cpu_cores =[int(core) for core in args.cpu_cores] gpu_devices = [int(device) for device in args.gpu_devices] - assert len(datasets) == len(train_configs), 'Must have same number of datasets as training configs!' + assert len(datasets) == len(train_configs), "Must have same number of datasets as training configs!" if len(base_model_dirs) > 0: - assert len(base_model_dirs) == len(datasets), 'Must have same number of base models for fine-tuning as datasets and training configs!' + assert len(base_model_dirs) == len(datasets), "Must have same number of base models for fine-tuning as datasets and training configs!" if len(split_names) < len(datasets): if len(split_names) == 1: - logger.warning('Using split "{}" for all datasets/configs...'.format(split_names[0])) + logger.warning("Using split "{}" for all datasets/configs...".format(split_names[0])) split_names *= len(datasets) else: raise ValueError("Can't have fewer splits that datasets/configs provided unless there is only one.") - # parse configs + # Parse configs. analysis_config = YamlConfig(analysis_config) train_configs = [YamlConfig(cfg) for cfg in train_configs] - # search + # Search. search = GQCNNSearch(analysis_config, train_configs, datasets, split_names, output_dir=output_dir, search_name=search_name, cpu_cores=cpu_cores, gpu_devices=gpu_devices, base_models=base_model_dirs) search.search() diff --git a/tools/plot_training_losses.py b/tools/plot_training_losses.py index cd93a738..b7b41a65 100644 --- a/tools/plot_training_losses.py +++ b/tools/plot_training_losses.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,8 +21,7 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" + Script to plot the various errors saved during training. Author @@ -27,11 +29,15 @@ Jeff Mahler Required Parameters ------------------------- +------------------- model_dir : str - Command line argument, the path to the model whose errors are to plotted. All plots and other metrics will - be saved to this directory. + Command line argument, the path to the model whose errors are to plotted. + All plots and other metrics will be saved to this directory. """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import os import sys @@ -39,28 +45,20 @@ import numpy as np from autolab_core import Logger +from gqcnn.utils import GeneralConstants, GQCNNFilenames -PCT_POS_VAL_FILENAME = 'pct_pos_val.npy' -TRAIN_LOSS_FILENAME = 'train_losses.npy' -TRAIN_ERRORS_FILENAME = 'train_errors.npy' -VAL_LOSS_FILENAME = 'val_losses.npy' -VAL_ERRORS_FILENAME = 'val_errors.npy' -TRAIN_ITERS_FILENAME = 'train_eval_iters.npy' -VAL_ITERS_FILENAME = 'val_eval_iters.npy' -WINDOW = 10 - -# set up logger -logger = Logger.get_logger('tools/plot_training_losses.py') +# Set up logger. +logger = Logger.get_logger("tools/plot_training_losses.py") -if __name__ == '__main__': +if __name__ == "__main__": result_dir = sys.argv[1] - train_errors_filename = os.path.join(result_dir, TRAIN_ERRORS_FILENAME) - val_errors_filename = os.path.join(result_dir, VAL_ERRORS_FILENAME) - train_iters_filename = os.path.join(result_dir, TRAIN_ITERS_FILENAME) - val_iters_filename = os.path.join(result_dir, VAL_ITERS_FILENAME) - pct_pos_val_filename = os.path.join(result_dir, PCT_POS_VAL_FILENAME) - train_losses_filename = os.path.join(result_dir, TRAIN_LOSS_FILENAME) - val_losses_filename = os.path.join(result_dir, VAL_LOSS_FILENAME) + train_errors_filename = os.path.join(result_dir, GQCNNFilenames.TRAIN_ERRORS) + val_errors_filename = os.path.join(result_dir, GQCNNFilenames.VAL_ERRORS) + train_iters_filename = os.path.join(result_dir, GQCNNFilenames.TRAIN_ITERS) + val_iters_filename = os.path.join(result_dir, GQCNNFilenames.VAL_ITERS) + pct_pos_val_filename = os.path.join(result_dir, GQCNNFilenames.PCT_POS_VAL) + train_losses_filename = os.path.join(result_dir, GQCNNFilenames.TRAIN_LOSS) + val_losses_filename = os.path.join(result_dir, GQCNNFilenames.VAL_LOSS) raw_train_errors = np.load(train_errors_filename) val_errors = np.load(val_errors_filename) @@ -79,7 +77,7 @@ val_errors = np.r_[pct_pos_val, val_errors] val_iters = np.r_[0, val_iters] - # window the training error + # Window the training error. i = 0 train_errors = [] train_losses = [] @@ -103,66 +101,66 @@ if pct_pos_val > 0: norm_final_val_error = val_errors[-1] / pct_pos_val - logger.info('TRAIN') - logger.info('Original Error {}'.format(train_errors[0])) - logger.info('Final Error {}'.format(train_errors[-1])) - logger.info('Orig loss {}'.format(train_losses[0])) - logger.info('Final loss {}'.format(train_losses[-1])) + logger.info("TRAIN") + logger.info("Original Error {}".format(train_errors[0])) + logger.info("Final Error {}".format(train_errors[-1])) + logger.info("Orig loss {}".format(train_losses[0])) + logger.info("Final loss {}".format(train_losses[-1])) - logger.info('VAL') - logger.info('Original error {}'.format(pct_pos_val)) - logger.info('Final error {}'.format(val_errors[-1])) - logger.info('Normalized error {}'.format(norm_final_val_error)) + logger.info("VAL") + logger.info("Original error {}".format(pct_pos_val)) + logger.info("Final error {}".format(val_errors[-1])) + logger.info("Normalized error {}".format(norm_final_val_error)) if val_losses is not None: - logger.info('Orig loss {}'.format(val_losses[0])) - logger.info('Final loss {}'.format(val_losses[-1])) + logger.info("Orig loss {}".format(val_losses[0])) + logger.info("Final loss {}".format(val_losses[-1])) plt.figure() - plt.plot(train_iters, train_errors, linewidth=4, color='b') - plt.plot(val_iters, val_errors, linewidth=4, color='g') + plt.plot(train_iters, train_errors, linewidth=4, color="b") + plt.plot(val_iters, val_errors, linewidth=4, color="g") plt.ylim(0, 100) - plt.legend(('Training (Minibatch)', 'Validation'), fontsize=15, loc='best') - plt.xlabel('Iteration', fontsize=15) - plt.ylabel('Error Rate', fontsize=15) + plt.legend(("Training (Minibatch)", "Validation"), fontsize=15, loc="best") + plt.xlabel("Iteration", fontsize=15) + plt.ylabel("Error Rate", fontsize=15) plt.figure() - plt.plot(train_iters, norm_train_errors, linewidth=4, color='b') - plt.plot(val_iters, norm_val_errors, linewidth=4, color='g') + plt.plot(train_iters, norm_train_errors, linewidth=4, color="b") + plt.plot(val_iters, norm_val_errors, linewidth=4, color="g") plt.ylim(0, 2.0) - plt.legend(('Training (Minibatch)', 'Validation'), fontsize=15, loc='best') - plt.xlabel('Iteration', fontsize=15) - plt.ylabel('Normalized Error Rate', fontsize=15) + plt.legend(("Training (Minibatch)", "Validation"), fontsize=15, loc="best") + plt.xlabel("Iteration", fontsize=15) + plt.ylabel("Normalized Error Rate", fontsize=15) train_losses[train_losses > 100.0] = 3.0 plt.figure() - plt.plot(train_iters, train_losses, linewidth=4, color='b') + plt.plot(train_iters, train_losses, linewidth=4, color="b") plt.ylim(0, 2.0) - plt.xlabel('Iteration', fontsize=15) - plt.ylabel('Training Loss', fontsize=15) + plt.xlabel("Iteration", fontsize=15) + plt.ylabel("Training Loss", fontsize=15) if val_losses is not None: val_losses[val_losses > 100.0] = 3.0 plt.figure() - plt.plot(val_iters, val_losses, linewidth=4, color='b') + plt.plot(val_iters, val_losses, linewidth=4, color="b") plt.ylim(0, 2.0) - plt.xlabel('Iteration', fontsize=15) - plt.ylabel('Validation Loss', fontsize=15) + plt.xlabel("Iteration", fontsize=15) + plt.ylabel("Validation Loss", fontsize=15) plt.show() plt.figure(figsize=(8,6)) - plt.plot(train_iters, train_errors, linewidth=4, color='b') - plt.plot(val_iters, val_errors, linewidth=4, color='g') + plt.plot(train_iters, train_errors, linewidth=4, color="b") + plt.plot(val_iters, val_errors, linewidth=4, color="g") plt.ylim(0, 100) - plt.legend(('Training (Minibatch)', 'Validation'), fontsize=15, loc='best') - plt.xlabel('Iteration', fontsize=15) - plt.ylabel('Error Rate', fontsize=15) - plt.savefig(os.path.join(result_dir, 'training_curve.jpg')) + plt.legend(("Training (Minibatch)", "Validation"), fontsize=15, loc="best") + plt.xlabel("Iteration", fontsize=15) + plt.ylabel("Error Rate", fontsize=15) + plt.savefig(os.path.join(result_dir, "training_curve.jpg")) plt.figure(figsize=(8,6)) - plt.plot(train_iters, norm_train_errors, linewidth=4, color='b') - plt.plot(val_iters, norm_val_errors, linewidth=4, color='g') + plt.plot(train_iters, norm_train_errors, linewidth=4, color="b") + plt.plot(val_iters, norm_val_errors, linewidth=4, color="g") plt.ylim(0, 2.0) - plt.legend(('Training (Minibatch)', 'Validation'), fontsize=15, loc='best') - plt.xlabel('Iteration', fontsize=15) - plt.ylabel('Normalized Error Rate', fontsize=15) - plt.savefig(os.path.join(result_dir, 'normalized_training_curve.jpg')) + plt.legend(("Training (Minibatch)", "Validation"), fontsize=15, loc="best") + plt.xlabel("Iteration", fontsize=15) + plt.ylabel("Normalized Error Rate", fontsize=15) + plt.savefig(os.path.join(result_dir, "normalized_training_curve.jpg")) diff --git a/tools/run_policy.py b/tools/run_policy.py index a60b4d82..c382bed1 100644 --- a/tools/run_policy.py +++ b/tools/run_policy.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,11 +21,16 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +Script to run saved policy output from user. +Author +------ +Jeff Mahler """ -""" -Script to run saved policy output from a user run -Author: Jeff Mahler -""" +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import os import random @@ -31,105 +39,105 @@ import numpy as np from autolab_core import RigidTransform, YamlConfig, Logger +from visualization import Visualizer2D as vis2d from gqcnn import RgbdImageState, ParallelJawGrasp from gqcnn import CrossEntropyRobustGraspingPolicy -from visualization import Visualizer2D as vis2d -# set up logger -logger = Logger.get_logger('tools/run_policy.py') +# Set up logger. +logger = Logger.get_logger("tools/run_policy.py") -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Run a saved test case through a GQ-CNN policy. For debugging purposes only.') - parser.add_argument('test_case_path', type=str, default=None, help='path to test case') - parser.add_argument('--config_filename', type=str, default='cfg/tools/run_policy.yaml', help='path to configuration file to use') - parser.add_argument('--output_dir', type=str, default=None, help='directory to store output') - parser.add_argument('--seed', type=int, default=None, help='random seed') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Run a saved test case through a GQ-CNN policy. For debugging purposes only.") + parser.add_argument("test_case_path", type=str, default=None, help="path to test case") + parser.add_argument("--config_filename", type=str, default="cfg/tools/run_policy.yaml", help="path to configuration file to use") + parser.add_argument("--output_dir", type=str, default=None, help="directory to store output") + parser.add_argument("--seed", type=int, default=None, help="random seed") args = parser.parse_args() test_case_path = args.test_case_path config_filename = args.config_filename output_dir = args.output_dir seed = args.seed - # make output dir + # Make output dir. if output_dir is not None and not os.path.exists(output_dir): os.mkdir(output_dir) - # make relative paths absolute + # Make relative paths absolute. if not os.path.isabs(config_filename): config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', + "..", config_filename) - # set random seed + # Set random seed. if seed is not None: np.random.seed(seed) random.seed(seed) - # read config + # Read config. config = YamlConfig(config_filename) - policy_config = config['policy'] + policy_config = config["policy"] - # load test case - state_path = os.path.join(test_case_path, 'state') - action_path = os.path.join(test_case_path, 'action') + # Load test case. + state_path = os.path.join(test_case_path, "state") + action_path = os.path.join(test_case_path, "action") state = RgbdImageState.load(state_path) original_action = ParallelJawGrasp.load(action_path) - # init policy + # Init policy. policy = CrossEntropyRobustGraspingPolicy(policy_config) - if policy_config['vis']['input_images']: + if policy_config["vis"]["input_images"]: vis2d.figure() if state.segmask is None: vis2d.subplot(1,2,1) vis2d.imshow(state.rgbd_im.color) - vis2d.title('COLOR') + vis2d.title("COLOR") vis2d.subplot(1,2,2) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config['vis']['vmin'], - vmax=policy_config['vis']['vmax']) - vis2d.title('DEPTH') + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) + vis2d.title("DEPTH") else: vis2d.subplot(1,3,1) vis2d.imshow(state.rgbd_im.color) - vis2d.title('COLOR') + vis2d.title("COLOR") vis2d.subplot(1,3,2) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config['vis']['vmin'], - vmax=policy_config['vis']['vmax']) - vis2d.title('DEPTH') + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) + vis2d.title("DEPTH") vis2d.subplot(1,3,3) vis2d.imshow(state.segmask) - vis2d.title('SEGMASK') + vis2d.title("SEGMASK") filename = None if output_dir is not None: - filename = os.path.join(output_dir, 'input_images.png') + filename = os.path.join(output_dir, "input_images.png") vis2d.show(filename) - # query policy + # Query policy. policy_start = time.time() action = policy(state) - logger.info('Planning took %.3f sec' %(time.time() - policy_start)) + logger.info("Planning took %.3f sec" %(time.time() - policy_start)) - # vis final grasp - if policy_config['vis']['final_grasp']: + # Vis final grasp. + if policy_config["vis"]["final_grasp"]: vis2d.figure(size=(10,10)) vis2d.subplot(1,2,1) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config['vis']['vmin'], - vmax=policy_config['vis']['vmax']) - vis2d.grasp(original_action.grasp, scale=policy_config['vis']['grasp_scale'], show_center=False, show_axis=True, color='r') - vis2d.title('Original (Q=%.3f) (Z=%.3f)' %(original_action.q_value, + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) + vis2d.grasp(original_action.grasp, scale=policy_config["vis"]["grasp_scale"], show_center=False, show_axis=True, color="r") + vis2d.title("Original (Q=%.3f) (Z=%.3f)" %(original_action.q_value, original_action.grasp.depth)) vis2d.subplot(1,2,2) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config['vis']['vmin'], - vmax=policy_config['vis']['vmax']) - vis2d.grasp(action.grasp, scale=policy_config['vis']['grasp_scale'], show_center=False, show_axis=True, color='r') - vis2d.title('New (Q=%.3f) (Z=%.3f)' %(action.q_value, + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) + vis2d.grasp(action.grasp, scale=policy_config["vis"]["grasp_scale"], show_center=False, show_axis=True, color="r") + vis2d.title("New (Q=%.3f) (Z=%.3f)" %(action.q_value, action.grasp.depth)) filename = None if output_dir is not None: - filename = os.path.join(output_dir, 'planned_grasp.png') + filename = os.path.join(output_dir, "planned_grasp.png") vis2d.show(filename) diff --git a/tools/train.py b/tools/train.py index 69c8a8bb..90e33824 100644 --- a/tools/train.py +++ b/tools/train.py @@ -1,12 +1,15 @@ # -*- coding: utf-8 -*- """ -Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved. -Permission to use, copy, modify, and distribute this software and its documentation for educational, -research, and not-for-profit purposes, without fee and without a signed licensing agreement, is -hereby granted, provided that the above copyright notice, this paragraph and the following two -paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology -Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643- -7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. +Copyright ©2017. The Regents of the University of California (Regents). +All Rights Reserved. Permission to use, copy, modify, and distribute this +software and its documentation for educational, research, and not-for-profit +purposes, without fee and without a signed licensing agreement, is hereby +granted, provided that the above copyright notice, this paragraph and the +following two paragraphs appear in all copies, modifications, and +distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +otl@berkeley.edu, +http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF @@ -18,46 +21,49 @@ PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -""" -Script for training a Grasp Quality Neural Network (GQ-CNN). + +Script for training a Grasp Quality Convolutional Neural Network (GQ-CNN). Author ------ Vishal Satish & Jeff Mahler """ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + import argparse import os import time -import autolab_core.utils as utils from autolab_core import YamlConfig, Logger +import autolab_core.utils as utils from gqcnn import get_gqcnn_model, get_gqcnn_trainer, utils as gqcnn_utils -# setup logger -logger = Logger.get_logger('tools/train.py') +# Setup logger. +logger = Logger.get_logger("tools/train.py") -if __name__ == '__main__': - # parse args - parser = argparse.ArgumentParser(description='Train a Grasp Quality Convolutional Neural Network with TensorFlow') - parser.add_argument('dataset_dir', type=str, default=None, - help='path to the dataset to use for training and validation') - parser.add_argument('--split_name', type=str, default='image_wise', - help='name of the split to train on') - parser.add_argument('--output_dir', type=str, default=None, - help='path to store the model') - parser.add_argument('--tensorboard_port', type=int, default=None, - help='port to launch tensorboard on') - parser.add_argument('--seed', type=int, default=None, - help='random seed for training') - parser.add_argument('--config_filename', type=str, default=None, - help='path to the configuration file to use') - parser.add_argument('--name', type=str, default=None, - help='name for the trained model') - parser.add_argument('--save_datetime', type=bool, default=False, - help='whether or not to save a model with the date and time of training') - parser.add_argument('--backend', type=str, default='tf', - help='the deep learning framework to use') +if __name__ == "__main__": + # Parse args. + parser = argparse.ArgumentParser(description="Train a Grasp Quality Convolutional Neural Network with TensorFlow") + parser.add_argument("dataset_dir", type=str, default=None, + help="path to the dataset to use for training and validation") + parser.add_argument("--split_name", type=str, default="image_wise", + help="name of the split to train on") + parser.add_argument("--output_dir", type=str, default=None, + help="path to store the model") + parser.add_argument("--tensorboard_port", type=int, default=None, + help="port to launch tensorboard on") + parser.add_argument("--seed", type=int, default=None, + help="random seed for training") + parser.add_argument("--config_filename", type=str, default=None, + help="path to the configuration file to use") + parser.add_argument("--name", type=str, default=None, + help="name for the trained model") + parser.add_argument("--save_datetime", type=bool, default=False, + help="whether or not to save a model with the date and time of training") + parser.add_argument("--backend", type=str, default="tf", + help="the deep learning framework to use") args = parser.parse_args() dataset_dir = args.dataset_dir split_name = args.split_name @@ -69,18 +75,18 @@ save_datetime = args.save_datetime backend = args.backend - # set default output dir + # Set default output dir. if output_dir is None: output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '../models') + "../models") - # set default config filename + # Set default config filename. if config_filename is None: config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', - 'cfg/train.yaml') + "..", + "cfg/train.yaml") - # turn relative paths absolute + # Turn relative paths absolute. if not os.path.isabs(dataset_dir): dataset_dir = os.path.join(os.getcwd(), dataset_dir) if not os.path.isabs(output_dir): @@ -88,30 +94,30 @@ if not os.path.isabs(config_filename): config_filename = os.path.join(os.getcwd(), config_filename) - # create output dir if necessary + # Create output dir if necessary. utils.mkdir_safe(output_dir) - # open train config + # Open train config. train_config = YamlConfig(config_filename) if seed is not None: - train_config['seed'] = seed - train_config['gqcnn']['seed'] = seed + train_config["seed"] = seed + train_config["gqcnn"]["seed"] = seed if tensorboard_port is not None: - train_config['tensorboard_port'] = tensorboard_port - gqcnn_params = train_config['gqcnn'] + train_config["tensorboard_port"] = tensorboard_port + gqcnn_params = train_config["gqcnn"] - # create a unique output folder based on the date and time + # Create a unique output folder based on the date and time. if save_datetime: - # create output dir + # Create output dir. unique_name = time.strftime("%Y%m%d-%H%M%S") output_dir = os.path.join(output_dir, unique_name) utils.mkdir_safe(output_dir) - # set visible devices - if 'gpu_list' in train_config: - gqcnn_utils.set_cuda_visible_devices(train_config['gpu_list']) + # Set visible devices. + if "gpu_list" in train_config: + gqcnn_utils.set_cuda_visible_devices(train_config["gpu_list"]) - # train the network + # Train the network. start_time = time.time() gqcnn = get_gqcnn_model(backend)(gqcnn_params) trainer = get_gqcnn_trainer(backend)(gqcnn, @@ -121,4 +127,4 @@ train_config, name=name) trainer.train() - logger.info('Total Training Time: ' + str(utils.get_elapsed_time(time.time() - start_time))) + logger.info("Total Training Time: " + str(utils.get_elapsed_time(time.time() - start_time))) From a663f4a773d7637fd43c93f265f40d7831fd1f8c Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 11:20:33 -0700 Subject: [PATCH 04/29] First round of YAPF and flake8. --- ci/travis/format.sh | 9 +- docs/source/conf.py | 43 +- examples/antipodal_grasp_sampling.py | 33 +- examples/policy.py | 185 ++-- examples/policy_ros.py | 113 ++- examples/policy_with_image_proc.py | 146 ++-- gqcnn/__init__.py | 11 +- gqcnn/analysis/analyzer.py | 545 +++++++----- gqcnn/grasping/__init__.py | 20 +- gqcnn/grasping/actions.py | 68 +- gqcnn/grasping/constraint_fn.py | 39 +- gqcnn/grasping/grasp.py | 214 +++-- gqcnn/grasping/grasp_quality_function.py | 619 +++++++------ gqcnn/grasping/image_grasp_sampler.py | 464 ++++++---- gqcnn/grasping/policy/__init__.py | 10 +- gqcnn/grasping/policy/enums.py | 6 +- gqcnn/grasping/policy/fc_policy.py | 294 +++++-- gqcnn/grasping/policy/policy.py | 702 +++++++++------ gqcnn/model/__init__.py | 24 +- gqcnn/model/tf/fc_network_tf.py | 256 ++++-- gqcnn/model/tf/network_tf.py | 846 +++++++++++------- gqcnn/search/enums.py | 20 +- gqcnn/search/resource_manager.py | 106 ++- gqcnn/search/search.py | 133 ++- gqcnn/search/trial.py | 115 ++- gqcnn/search/utils.py | 106 ++- gqcnn/training/__init__.py | 10 +- gqcnn/training/tf/trainer_tf.py | 1010 ++++++++++++++-------- gqcnn/utils/__init__.py | 23 +- gqcnn/utils/enums.py | 23 +- gqcnn/utils/policy_exceptions.py | 17 +- gqcnn/utils/train_stats_logger.py | 66 +- gqcnn/utils/utils.py | 70 +- ros_nodes/grasp_planner_node.py | 229 +++-- setup.py | 123 +-- tools/analyze_gqcnn_performance.py | 52 +- tools/finetune.py | 75 +- tools/hyperparam_search.py | 81 +- tools/plot_training_losses.py | 33 +- tools/run_policy.py | 91 +- tools/train.py | 72 +- 41 files changed, 4510 insertions(+), 2592 deletions(-) mode change 100644 => 100755 ci/travis/format.sh diff --git a/ci/travis/format.sh b/ci/travis/format.sh old mode 100644 new mode 100755 index d07ada15..f8fe53fb --- a/ci/travis/format.sh +++ b/ci/travis/format.sh @@ -3,7 +3,7 @@ # Script for YAPF formatting. Adapted from https://github.com/ray-project/ray/blob/master/ci/travis/format.sh. YAPF_FLAGS=( - '--style' "$ROOT/.style.yapf" + '--style' ".style.yapf" '--recursive' '--parallel' ) @@ -15,12 +15,12 @@ format() { yapf --in-place "${YAPF_FLAGS[@]}" -- "$@" } -# Format all files, and print the diff to stdout for travis. +# Format all files, and print the diff to `stdout` for Travis. format_all() { - yapf --diff "${YAPF_FLAGS[@]}" "${YAPF_EXCLUDES[@]}" test python + yapf --diff "${YAPF_FLAGS[@]}" "${YAPF_EXCLUDES[@]}" } -# This flag formats individual files. --files *must* be the first command line +# This flag formats individual files. `--files` *must* be the first command line # arg to use this option. if [[ "$1" == '--files' ]]; then format "${@:2}" @@ -28,3 +28,4 @@ if [[ "$1" == '--files' ]]; then # entire Python directory is formatted. elif [[ "$1" == '--all' ]]; then format_all +fi diff --git a/docs/source/conf.py b/docs/source/conf.py index 4ec577a3..43672e6f 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -19,9 +19,13 @@ # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. -sys.path.insert(0, os.path.join(os.path.abspath(os.path.dirname(__name__)), '../../')) -sys.path.insert(0, os.path.join(os.path.abspath(os.path.dirname(__name__)), '../../examples')) -sys.path.insert(0, os.path.join(os.path.abspath(os.path.dirname(__name__)), '../../tools')) +sys.path.insert( + 0, os.path.join(os.path.abspath(os.path.dirname(__name__)), '../../')) +sys.path.insert( + 0, + os.path.join(os.path.abspath(os.path.dirname(__name__)), '../../examples')) +sys.path.insert( + 0, os.path.join(os.path.abspath(os.path.dirname(__name__)), '../../tools')) # -- General configuration ------------------------------------------------ @@ -31,9 +35,7 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = [ - 'sphinx.ext.autodoc', 'sphinxcontrib.napoleon' -] +extensions = ['sphinx.ext.autodoc', 'sphinxcontrib.napoleon'] autoclass_content = 'class' autodoc_member_order = 'bysource' autodoc_default_flags = ['members', 'show-inheritance'] @@ -112,7 +114,6 @@ # If true, `todo` and `todoList` produce output, else they produce nothing. todo_include_todos = False - # -- Options for HTML output ---------------------------------------------- # The theme to use for HTML and HTML Help pages. See the documentation for @@ -215,17 +216,17 @@ # -- Options for LaTeX output --------------------------------------------- latex_elements = { -# The paper size ('letterpaper' or 'a4paper'). -#'papersize': 'letterpaper', + # The paper size ('letterpaper' or 'a4paper'). + #'papersize': 'letterpaper', -# The font size ('10pt', '11pt' or '12pt'). -#'pointsize': '10pt', + # The font size ('10pt', '11pt' or '12pt'). + #'pointsize': '10pt', -# Additional stuff for the LaTeX preamble. -#'preamble': '', + # Additional stuff for the LaTeX preamble. + #'preamble': '', -# Latex figure (float) alignment -#'figure_align': 'htbp', + # Latex figure (float) alignment + #'figure_align': 'htbp', } # Grouping the document tree into LaTeX files. List of tuples @@ -256,29 +257,23 @@ # If false, no module index is generated. #latex_domain_indices = True - # -- Options for manual page output --------------------------------------- # One entry per manual page. List of tuples # (source start file, name, description, authors, manual section). -man_pages = [ - (master_doc, 'GQCNN', u'GQCNN Documentation', - [author], 1) -] +man_pages = [(master_doc, 'GQCNN', u'GQCNN Documentation', [author], 1)] # If true, show URL addresses after external links. #man_show_urls = False - # -- Options for Texinfo output ------------------------------------------- # Grouping the document tree into Texinfo files. List of tuples # (source start file, target name, title, author, # dir menu entry, description, category) texinfo_documents = [ - (master_doc, 'GQCNN', u'GQCNN Documentation', - author, 'GQCNN', 'One line description of project.', - 'Miscellaneous'), + (master_doc, 'GQCNN', u'GQCNN Documentation', author, 'GQCNN', + 'One line description of project.', 'Miscellaneous'), ] # Documents to append as an appendix to all manuals. diff --git a/examples/antipodal_grasp_sampling.py b/examples/antipodal_grasp_sampling.py index 66f53046..30654d60 100644 --- a/examples/antipodal_grasp_sampling.py +++ b/examples/antipodal_grasp_sampling.py @@ -25,18 +25,17 @@ Demonstrates image-based antipodal grasp candidate sampling, which is used in the Cross Entropy Method (CEM)-based GQ-CNN grasping policy. Samples images from a BerkeleyAutomation/perception `RgbdSensor`. -Author: Jeff Mahler + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division from __future__ import print_function - import argparse import os -import sys - -import numpy as np from autolab_core import RigidTransform, YamlConfig, Logger from perception import RgbdImage, RgbdSensorFactory @@ -49,8 +48,12 @@ if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Sample antipodal grasps on a depth image from an RgbdSensor") - parser.add_argument("--config_filename", type=str, default="cfg/examples/antipodal_grasp_sampling.yaml", help="path to configuration file to use") + parser = argparse.ArgumentParser(description=( + "Sample antipodal grasps on a depth image from an RgbdSensor")) + parser.add_argument("--config_filename", + type=str, + default="cfg/examples/antipodal_grasp_sampling.yaml", + help="path to configuration file to use") args = parser.parse_args() config_filename = args.config_filename @@ -65,8 +68,9 @@ sample_config = config["sampling"] # Read camera calib. - tf_filename = "%s_to_world.tf" %(sensor_frame) - T_camera_world = RigidTransform.load(os.path.join(config["calib_dir"], sensor_frame, tf_filename)) + tf_filename = "%s_to_world.tf" % (sensor_frame) + T_camera_world = RigidTransform.load( + os.path.join(config["calib_dir"], sensor_frame, tf_filename)) # Setup sensor. sensor = RgbdSensorFactory.sensor(sensor_type, config["sensor"]) @@ -80,9 +84,14 @@ rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) # Sample grasps. - grasp_sampler = AntipodalDepthImageGraspSampler(sample_config, gripper_width) - grasps = grasp_sampler.sample(rgbd_im, camera_intr, num_grasp_samples, segmask=None, - seed=100, visualize=visualize_sampling) + grasp_sampler = AntipodalDepthImageGraspSampler(sample_config, + gripper_width) + grasps = grasp_sampler.sample(rgbd_im, + camera_intr, + num_grasp_samples, + segmask=None, + seed=100, + visualize=visualize_sampling) # Visualize. vis.figure() diff --git a/examples/policy.py b/examples/policy.py index b053ddce..31c67a52 100644 --- a/examples/policy.py +++ b/examples/policy.py @@ -25,8 +25,11 @@ Displays robust grasps planned using a GQ-CNN grapsing policy on a set of saved RGB-D images. The default configuration for the standard GQ-CNN policy is `cfg/examples/cfg/examples/gqcnn_pj.yaml`. The default configuration for the -Fully-Convolutional GQ-CNN policy is cfg/examples/fc_gqcnn_pj.yaml. -Author: Jeff Mahler, Vishal Satish +Fully-Convolutional GQ-CNN policy is `cfg/examples/fc_gqcnn_pj.yaml`. + +Author +------ +Jeff Mahler & Vishal Satish """ from __future__ import absolute_import from __future__ import division @@ -39,26 +42,54 @@ import numpy as np -from autolab_core import RigidTransform, YamlConfig, Logger -from perception import BinaryImage, CameraIntrinsics, ColorImage, DepthImage, RgbdImage +from autolab_core import YamlConfig, Logger +from perception import (BinaryImage, CameraIntrinsics, ColorImage, DepthImage, + RgbdImage) from visualization import Visualizer2D as vis -from gqcnn.grasping import RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, RgbdImageState, FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction -from gqcnn.utils import GripperMode, NoValidGraspsException +from gqcnn.grasping import (RobustGraspingPolicy, + CrossEntropyRobustGraspingPolicy, RgbdImageState, + FullyConvolutionalGraspingPolicyParallelJaw, + FullyConvolutionalGraspingPolicySuction) +from gqcnn.utils import GripperMode # Set up logger. logger = Logger.get_logger("examples/policy.py") if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Run a grasping policy on an example image") - parser.add_argument("model_name", type=str, default=None, help="name of a trained model to run") - parser.add_argument("--depth_image", type=str, default=None, help="path to a test depth image stored as a .npy file") - parser.add_argument("--segmask", type=str, default=None, help="path to an optional segmask to use") - parser.add_argument("--camera_intr", type=str, default=None, help="path to the camera intrinsics") - parser.add_argument("--model_dir", type=str, default=None, help="path to the folder in which the model is stored") - parser.add_argument("--config_filename", type=str, default=None, help="path to configuration file to use") - parser.add_argument("--fully_conv", action="store_true", help="run Fully-Convolutional GQ-CNN policy instead of standard GQ-CNN policy") + parser = argparse.ArgumentParser( + description="Run a grasping policy on an example image") + parser.add_argument("model_name", + type=str, + default=None, + help="name of a trained model to run") + parser.add_argument( + "--depth_image", + type=str, + default=None, + help="path to a test depth image stored as a .npy file") + parser.add_argument("--segmask", + type=str, + default=None, + help="path to an optional segmask to use") + parser.add_argument("--camera_intr", + type=str, + default=None, + help="path to the camera intrinsics") + parser.add_argument("--model_dir", + type=str, + default=None, + help="path to the folder in which the model is stored") + parser.add_argument("--config_filename", + type=str, + default=None, + help="path to configuration file to use") + parser.add_argument( + "--fully_conv", + action="store_true", + help=("run Fully-Convolutional GQ-CNN policy instead of standard" + " GQ-CNN policy")) args = parser.parse_args() model_name = args.model_name depth_im_filename = args.depth_image @@ -68,75 +99,80 @@ config_filename = args.config_filename fully_conv = args.fully_conv - assert not (fully_conv and depth_im_filename is not None and segmask_filename is None), "Fully-Convolutional policy expects a segmask." + assert not (fully_conv and depth_im_filename is not None + and segmask_filename is None + ), "Fully-Convolutional policy expects a segmask." if depth_im_filename is None: if fully_conv: - depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/examples/clutter/primesense/depth_0.npy") + depth_im_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/examples/clutter/primesense/depth_0.npy") else: - depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/examples/single_object/primesense/depth_0.npy") + depth_im_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/examples/single_object/primesense/depth_0.npy") if fully_conv and segmask_filename is None: - segmask_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/examples/clutter/primesense/segmask_0.png") + segmask_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/examples/clutter/primesense/segmask_0.png") if camera_intr_filename is None: - camera_intr_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/calib/primesense/primesense.intr") - + camera_intr_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/calib/primesense/primesense.intr") - # Set model if provided. + # Set model if provided. if model_dir is None: model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../models") model_path = os.path.join(model_dir, model_name) # Get configs. - model_config = json.load(open(os.path.join(model_path, "config.json"), "r")) + model_config = json.load(open(os.path.join(model_path, "config.json"), + "r")) try: gqcnn_config = model_config["gqcnn"] gripper_mode = gqcnn_config["gripper_mode"] - except: + except KeyError: gqcnn_config = model_config["gqcnn_config"] input_data_mode = gqcnn_config["input_data_mode"] if input_data_mode == "tf_image": gripper_mode = GripperMode.LEGACY_PARALLEL_JAW elif input_data_mode == "tf_image_suction": - gripper_mode = GripperMode.LEGACY_SUCTION + gripper_mode = GripperMode.LEGACY_SUCTION elif input_data_mode == "suction": - gripper_mode = GripperMode.SUCTION + gripper_mode = GripperMode.SUCTION elif input_data_mode == "multi_suction": - gripper_mode = GripperMode.MULTI_SUCTION + gripper_mode = GripperMode.MULTI_SUCTION elif input_data_mode == "parallel_jaw": gripper_mode = GripperMode.PARALLEL_JAW else: - raise ValueError("Input data mode {} not supported!".format(input_data_mode)) - + raise ValueError( + "Input data mode {} not supported!".format(input_data_mode)) + # Set config. if config_filename is None: - if gripper_mode == GripperMode.LEGACY_PARALLEL_JAW or gripper_mode == GripperMode.PARALLEL_JAW: + if (gripper_mode == GripperMode.LEGACY_PARALLEL_JAW + or gripper_mode == GripperMode.PARALLEL_JAW): if fully_conv: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/fc_gqcnn_pj.yaml") + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/examples/fc_gqcnn_pj.yaml") else: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/gqcnn_pj.yaml") - elif gripper_mode == GripperMode.LEGACY_SUCTION or gripper_mode == GripperMode.SUCTION: + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/examples/gqcnn_pj.yaml") + elif (gripper_mode == GripperMode.LEGACY_SUCTION + or gripper_mode == GripperMode.SUCTION): if fully_conv: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/fc_gqcnn_suction.yaml") + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/examples/fc_gqcnn_suction.yaml") else: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/gqcnn_suction.yaml") - + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/examples/gqcnn_suction.yaml") + # Read config. config = YamlConfig(config_filename) inpaint_rescale_factor = config["inpaint_rescale_factor"] @@ -146,19 +182,20 @@ if "gqcnn_model" in policy_config["metric"]: policy_config["metric"]["gqcnn_model"] = model_path if not os.path.isabs(policy_config["metric"]["gqcnn_model"]): - policy_config["metric"]["gqcnn_model"] = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - policy_config["metric"]["gqcnn_model"]) - + policy_config["metric"]["gqcnn_model"] = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + policy_config["metric"]["gqcnn_model"]) + # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) - + # Read images. depth_data = np.load(depth_im_filename) depth_im = DepthImage(depth_data, frame=camera_intr.frame) - color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), + color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, + 3]).astype(np.uint8), frame=camera_intr.frame) - + # Optionally read a segmask. segmask = None if segmask_filename is not None: @@ -168,30 +205,33 @@ segmask = valid_px_mask else: segmask = segmask.mask_binary(valid_px_mask) - + # Inpaint. depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) - - if "input_images" in policy_config["vis"] and policy_config["vis"]["input_images"]: - vis.figure(size=(10,10)) + + if "input_images" in policy_config["vis"] and policy_config["vis"][ + "input_images"]: + vis.figure(size=(10, 10)) num_plot = 1 if segmask is not None: num_plot = 2 - vis.subplot(1,num_plot,1) + vis.subplot(1, num_plot, 1) vis.imshow(depth_im) if segmask is not None: - vis.subplot(1,num_plot,2) + vis.subplot(1, num_plot, 2) vis.imshow(segmask) vis.show() - + # Create state. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) # Set input sizes for fully-convolutional policy. if fully_conv: - policy_config["metric"]["fully_conv_gqcnn_config"]["im_height"] = depth_im.shape[0] - policy_config["metric"]["fully_conv_gqcnn_config"]["im_width"] = depth_im.shape[1] + policy_config["metric"]["fully_conv_gqcnn_config"][ + "im_height"] = depth_im.shape[0] + policy_config["metric"]["fully_conv_gqcnn_config"][ + "im_width"] = depth_im.shape[1] # Init policy. if fully_conv: @@ -201,7 +241,9 @@ elif policy_config["type"] == "fully_conv_pj": policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_config) else: - raise ValueError("Invalid fully-convolutional policy type: {}".format(policy_config["type"])) + raise ValueError( + "Invalid fully-convolutional policy type: {}".format( + policy_config["type"])) else: policy_type = "cem" if "type" in policy_config: @@ -216,14 +258,15 @@ # Query policy. policy_start = time.time() action = policy(state) - logger.info("Planning took %.3f sec" %(time.time() - policy_start)) + logger.info("Planning took %.3f sec" % (time.time() - policy_start)) # Vis final grasp. if policy_config["vis"]["final_grasp"]: - vis.figure(size=(10,10)) + vis.figure(size=(10, 10)) vis.imshow(rgbd_im.depth, vmin=policy_config["vis"]["vmin"], vmax=policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) - vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format(action.grasp.depth, action.q_value)) + vis.title("Planned grasp at depth {0:.3f}m with Q={1:.3f}".format( + action.grasp.depth, action.q_value)) vis.show() diff --git a/examples/policy_ros.py b/examples/policy_ros.py index f4c7da3e..db9e41d5 100644 --- a/examples/policy_ros.py +++ b/examples/policy_ros.py @@ -22,9 +22,12 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -Displays robust grasps planned using a GQ-CNN-based policy on a set of saved RGB-D images. -The default configuration is cfg/examples/policy.yaml. -Author: Jeff Mahler +Displays robust grasps planned using a GQ-CNN-based policy on a set of saved +RGB-D images. The default configuration is cfg/examples/policy.yaml. + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -37,31 +40,49 @@ import rosgraph.roslogging as rl import rospy import sys -import time from cv_bridge import CvBridge, CvBridgeError -from sensor_msgs.msg import Image, CameraInfo -from autolab_core import Point, RigidTransform, YamlConfig -from perception import BinaryImage, CameraIntrinsics, ColorImage, DepthImage, RgbdImage +from autolab_core import Point, Logger +from perception import BinaryImage, CameraIntrinsics, ColorImage, DepthImage from visualization import Visualizer2D as vis -from gqcnn.grasping import CrossEntropyRobustGraspingPolicy, RgbdImageState, Grasp2D, SuctionPoint2D, GraspAction -from gqcnn.msg import GQCNNGrasp, BoundingBox -from gqcnn.srv import GQCNNGraspPlanner, GQCNNGraspPlannerBoundingBox, GQCNNGraspPlannerSegmask +from gqcnn.grasping import Grasp2D, SuctionPoint2D, GraspAction +from gqcnn.msg import GQCNNGrasp +from gqcnn.srv import GQCNNGraspPlanner, GQCNNGraspPlannerSegmask # Set up logger. logger = Logger.get_logger("examples/policy_ros.py") if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Run a grasping policy on an example image") - parser.add_argument("--depth_image", type=str, default=None, help="path to a test depth image stored as a .npy file") - parser.add_argument("--segmask", type=str, default=None, help="path to an optional segmask to use") - parser.add_argument("--camera_intr", type=str, default=None, help="path to the camera intrinsics") - parser.add_argument("--gripper_width", type=float, default=0.05, help="width of the gripper to plan for") - parser.add_argument("--namespace", type=str, default="gqcnn", help="namespace of the ROS grasp planning service") - parser.add_argument("--vis_grasp", type=bool, default=True, help="whether or not to visualize the grasp") + parser = argparse.ArgumentParser( + description="Run a grasping policy on an example image") + parser.add_argument( + "--depth_image", + type=str, + default=None, + help="path to a test depth image stored as a .npy file") + parser.add_argument("--segmask", + type=str, + default=None, + help="path to an optional segmask to use") + parser.add_argument("--camera_intr", + type=str, + default=None, + help="path to the camera intrinsics") + parser.add_argument("--gripper_width", + type=float, + default=0.05, + help="width of the gripper to plan for") + parser.add_argument("--namespace", + type=str, + default="gqcnn", + help="namespace of the ROS grasp planning service") + parser.add_argument("--vis_grasp", + type=bool, + default=True, + help="whether or not to visualize the grasp") args = parser.parse_args() depth_im_filename = args.depth_image segmask_filename = args.segmask @@ -76,42 +97,42 @@ # Setup filenames. if depth_im_filename is None: - depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/examples/single_object/primesense/depth_0.npy") + depth_im_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/examples/single_object/primesense/depth_0.npy") if camera_intr_filename is None: - camera_intr_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/calib/primesense/primesense.intr") + camera_intr_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/calib/primesense/primesense.intr") # Wait for grasp planning service and create service proxy. - rospy.wait_for_service("%s/grasp_planner" %(namespace)) - rospy.wait_for_service("%s/grasp_planner_segmask" %(namespace)) - plan_grasp = rospy.ServiceProxy("%s/grasp_planner" %(namespace), GQCNNGraspPlanner) - plan_grasp_segmask = rospy.ServiceProxy("%s/grasp_planner_segmask" %(namespace), GQCNNGraspPlannerSegmask) - cv_bridge = CvBridge() + rospy.wait_for_service("%s/grasp_planner" % (namespace)) + rospy.wait_for_service("%s/grasp_planner_segmask" % (namespace)) + plan_grasp = rospy.ServiceProxy("%s/grasp_planner" % (namespace), + GQCNNGraspPlanner) + plan_grasp_segmask = rospy.ServiceProxy( + "%s/grasp_planner_segmask" % (namespace), GQCNNGraspPlannerSegmask) + cv_bridge = CvBridge() # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) - + # Read images. depth_im = DepthImage.open(depth_im_filename, frame=camera_intr.frame) - color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), + color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, + 3]).astype(np.uint8), frame=camera_intr.frame) # Read segmask. if segmask_filename is not None: segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame) - grasp_resp = plan_grasp_segmask(color_im.rosmsg, - depth_im.rosmsg, - camera_intr.rosmsg, - segmask.rosmsg) + grasp_resp = plan_grasp_segmask(color_im.rosmsg, depth_im.rosmsg, + camera_intr.rosmsg, segmask.rosmsg) else: - grasp_resp = plan_grasp(color_im.rosmsg, - depth_im.rosmsg, + grasp_resp = plan_grasp(color_im.rosmsg, depth_im.rosmsg, camera_intr.rosmsg) grasp = grasp_resp.grasp - + # Convert to a grasp action. grasp_type = grasp.grasp_type if grasp_type == GQCNNGrasp.PARALLEL_JAW: @@ -126,25 +147,25 @@ center = Point(np.array([grasp.center_px[0], grasp.center_px[1]]), frame=camera_intr.frame) grasp_2d = SuctionPoint2D(center, - np.array([0,0,1]), + np.array([0, 0, 1]), grasp.depth, - camera_intr=camera_intr) + camera_intr=camera_intr) else: - raise ValueError("Grasp type %d not recognized!" %(grasp_type)) + raise ValueError("Grasp type %d not recognized!" % (grasp_type)) try: - thumbnail = DepthImage(cv_bridge.imgmsg_to_cv2(grasp.thumbnail, - desired_encoding="passthrough"), + thumbnail = DepthImage(cv_bridge.imgmsg_to_cv2( + grasp.thumbnail, desired_encoding="passthrough"), frame=camera_intr.frame) - except CVBridgeError as e: + except CvBridgeError as e: logger.error(e) logger.error("Failed to convert image") sys.exit(1) action = GraspAction(grasp_2d, grasp.q_value, thumbnail) - + # Vis final grasp. if vis_grasp: - vis.figure(size=(10,10)) + vis.figure(size=(10, 10)) vis.imshow(depth_im, vmin=0.6, vmax=0.9) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) - vis.title("Planned grasp on depth (Q=%.3f)" %(action.q_value)) + vis.title("Planned grasp on depth (Q=%.3f)" % (action.q_value)) vis.show() diff --git a/examples/policy_with_image_proc.py b/examples/policy_with_image_proc.py index d730cd64..43eee40a 100644 --- a/examples/policy_with_image_proc.py +++ b/examples/policy_with_image_proc.py @@ -22,9 +22,12 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -Displays robust grasps planned using a GQ-CNN-based policy on a set of saved RGB-D images. -The default configuration is cfg/examples/policy.yaml. -Author: Jeff Mahler +Displays robust grasps planned using a GQ-CNN-based policy on a set of saved +RGB-D images. The default configuration is cfg/examples/policy.yaml. + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -39,10 +42,12 @@ import skimage from autolab_core import PointCloud, RigidTransform, YamlConfig, Logger -from perception import BinaryImage, CameraIntrinsics, ColorImage, DepthImage, RgbdImage, SegmentationImage +from perception import (BinaryImage, CameraIntrinsics, ColorImage, DepthImage, + RgbdImage, SegmentationImage) from visualization import Visualizer2D as vis -from gqcnn import RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, RgbdImageState +from gqcnn import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, + RgbdImageState) CLUSTER_TOL = 0.0015 MIN_CLUSTER_SIZE = 100 @@ -53,13 +58,33 @@ if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Run a grasping policy on an example image") - parser.add_argument("--depth_image", type=str, default=None, help="path to a test depth image stored as a .npy file") - parser.add_argument("--segmask", type=str, default=None, help="path to an optional segmask to use") - parser.add_argument("--camera_intrinsics", type=str, default=None, help="path to the camera intrinsics") - parser.add_argument("--camera_pose", type=str, default=None, help="path to the camera pose") - parser.add_argument("--model_dir", type=str, default=None, help="path to a trained model to run") - parser.add_argument("--config_filename", type=str, default=None, help="path to configuration file to use") + parser = argparse.ArgumentParser( + description="Run a grasping policy on an example image") + parser.add_argument( + "--depth_image", + type=str, + default=None, + help="path to a test depth image stored as a .npy file") + parser.add_argument("--segmask", + type=str, + default=None, + help="path to an optional segmask to use") + parser.add_argument("--camera_intrinsics", + type=str, + default=None, + help="path to the camera intrinsics") + parser.add_argument("--camera_pose", + type=str, + default=None, + help="path to the camera pose") + parser.add_argument("--model_dir", + type=str, + default=None, + help="path to a trained model to run") + parser.add_argument("--config_filename", + type=str, + default=None, + help="path to configuration file to use") args = parser.parse_args() depth_im_filename = args.depth_image segmask_filename = args.segmask @@ -69,22 +94,22 @@ config_filename = args.config_filename if depth_im_filename is None: - depth_im_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/examples/single_object/depth_0.npy") + depth_im_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/examples/single_object/depth_0.npy") if camera_intr_filename is None: - camera_intr_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/calib/primesense.intr") + camera_intr_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/calib/primesense.intr") if camera_pose_filename is None: - camera_pose_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "data/calib/primesense.tf") + camera_pose_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "data/calib/primesense.tf") if config_filename is None: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/replication/dex-net_2.0.yaml") - + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/examples/replication/dex-net_2.0.yaml") + # Read config. config = YamlConfig(config_filename) inpaint_rescale_factor = config["inpaint_rescale_factor"] @@ -93,25 +118,26 @@ # Make relative paths absolute. if model_dir is not None: policy_config["metric"]["gqcnn_model"] = model_dir - if "gqcnn_model" in policy_config["metric"] and not os.path.isabs(policy_config["metric"]["gqcnn_model"]): - policy_config["metric"]["gqcnn_model"] = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - policy_config["metric"]["gqcnn_model"]) + if "gqcnn_model" in policy_config["metric"] and not os.path.isabs( + policy_config["metric"]["gqcnn_model"]): + policy_config["metric"]["gqcnn_model"] = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + policy_config["metric"]["gqcnn_model"]) # Setup sensor. camera_intr = CameraIntrinsics.load(camera_intr_filename) T_camera_world = RigidTransform.load(camera_pose_filename) - + # Read images. depth_data = np.load(depth_im_filename) depth_data = depth_data.astype(np.float32) / 1000.0 depth_im = DepthImage(depth_data, frame=camera_intr.frame) - color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, 3]).astype(np.uint8), + color_im = ColorImage(np.zeros([depth_im.height, depth_im.width, + 3]).astype(np.uint8), frame=camera_intr.frame) - + # Optionally read a segmask. - mask = np.zeros( - (camera_intr.height, camera_intr.width, 1), dtype=np.uint8) + mask = np.zeros((camera_intr.height, camera_intr.width, 1), dtype=np.uint8) c = np.array([165, 460, 500, 135]) r = np.array([165, 165, 370, 370]) rr, cc = skimage.draw.polygon(r, c, shape=mask.shape) @@ -141,46 +167,51 @@ num_clusters = len(cluster_indices) obj_segmask_data = np.zeros(depth_im.shape) - + # Read out all points in large clusters. cur_i = 0 for j, indices in enumerate(cluster_indices): num_points = len(indices) - points = np.zeros([3,num_points]) - + points = np.zeros([3, num_points]) + for i, index in enumerate(indices): - points[0,i] = pcl_cloud[index][0] - points[1,i] = pcl_cloud[index][1] - points[2,i] = pcl_cloud[index][2] - + points[0, i] = pcl_cloud[index][0] + points[1, i] = pcl_cloud[index][1] + points[2, i] = pcl_cloud[index][2] + segment = PointCloud(points, frame=camera_intr.frame) depth_segment = camera_intr.project_to_image(segment) - obj_segmask_data[depth_segment.data > 0] = j+1 + obj_segmask_data[depth_segment.data > 0] = j + 1 obj_segmask = SegmentationImage(obj_segmask_data.astype(np.uint8)) obj_segmask = obj_segmask.mask_binary(segmask) # Inpaint. depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) - - if "input_images" in policy_config["vis"] and policy_config["vis"]["input_images"]: - vis.figure(size=(10,10)) + + if "input_images" in policy_config["vis"] and policy_config["vis"][ + "input_images"]: + vis.figure(size=(10, 10)) num_plot = 3 - vis.subplot(1,num_plot,1) + vis.subplot(1, num_plot, 1) vis.imshow(depth_im) - vis.subplot(1,num_plot,2) + vis.subplot(1, num_plot, 2) vis.imshow(segmask) - vis.subplot(1,num_plot,3) + vis.subplot(1, num_plot, 3) vis.imshow(obj_segmask) vis.show() - - from visualization import Visualizer3D as vis3d + + from visualization import Visualizer3D as vis3d point_cloud = camera_intr.deproject(depth_im) vis3d.figure() - vis3d.points(point_cloud, subsample=3, random=True, color=(0,0,1), scale=0.001) + vis3d.points(point_cloud, + subsample=3, + random=True, + color=(0, 0, 1), + scale=0.001) vis3d.pose(RigidTransform()) vis3d.pose(T_camera_world.inverse()) vis3d.show() - + # Create state. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) @@ -195,20 +226,21 @@ policy = CrossEntropyRobustGraspingPolicy(policy_config) policy_start = time.time() action = policy(state) - logger.info("Planning took %.3f sec" %(time.time() - policy_start)) + logger.info("Planning took %.3f sec" % (time.time() - policy_start)) # Vis final grasp. if policy_config["vis"]["final_grasp"]: - vis.figure(size=(10,10)) + vis.figure(size=(10, 10)) vis.imshow(rgbd_im.depth, vmin=policy_config["vis"]["vmin"], vmax=policy_config["vis"]["vmax"]) vis.grasp(action.grasp, scale=2.5, show_center=False, show_axis=True) - vis.title("Planned grasp on depth (Q=%.3f)" %(action.q_value)) + vis.title("Planned grasp on depth (Q=%.3f)" % (action.q_value)) vis.show() # Get grasp pose. - T_grasp_camera = action.grasp.pose(grasp_approach_dir=-T_camera_world.inverse().z_axis) + T_grasp_camera = action.grasp.pose( + grasp_approach_dir=-T_camera_world.inverse().z_axis) grasp_pose_msg = T_grasp_camera.pose_msg - + # TODO: Control to reach the grasp pose. diff --git a/gqcnn/__init__.py b/gqcnn/__init__.py index 2303fde3..59c380bf 100644 --- a/gqcnn/__init__.py +++ b/gqcnn/__init__.py @@ -26,7 +26,7 @@ from __future__ import division from __future__ import print_function -from .model import get_gqcnn_model, get_fc_gqcnn_model +from .model import get_gqcnn_model, get_fc_gqcnn_model from .training import get_gqcnn_trainer from .grasping import (RobustGraspingPolicy, UniformRandomGraspingPolicy, CrossEntropyRobustGraspingPolicy, RgbdImageState, @@ -36,4 +36,11 @@ from .search import GQCNNSearch from .utils import NoValidGraspsException, NoAntipodalPairsFoundException -__all__ = ["get_gqcnn_model", "get_fc_gqcnn_model", "get_gqcnn_trainer", "RobustGraspingPolicy", "UniformRandomGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "RgbdImageState","FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "GQCNNAnalyzer", "GQCNNSearch", "NoValidGraspsException", "NoAntipodalPairsFoundException"] +__all__ = [ + "get_gqcnn_model", "get_fc_gqcnn_model", "get_gqcnn_trainer", + "RobustGraspingPolicy", "UniformRandomGraspingPolicy", + "CrossEntropyRobustGraspingPolicy", "RgbdImageState", + "FullyConvolutionalGraspingPolicyParallelJaw", + "FullyConvolutionalGraspingPolicySuction", "GQCNNAnalyzer", "GQCNNSearch", + "NoValidGraspsException", "NoAntipodalPairsFoundException" +] diff --git a/gqcnn/analysis/analyzer.py b/gqcnn/analysis/analyzer.py index 1d7bced9..b2d5e0ef 100644 --- a/gqcnn/analysis/analyzer.py +++ b/gqcnn/analysis/analyzer.py @@ -23,39 +23,37 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Class for analyzing a GQ-CNN model for grasp quality prediction. -Author: Jeff Mahler + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division from __future__ import print_function -import copy import json import logging import os -import pickle as pkl -import random -import sys -import time -import matplotlib import matplotlib.pyplot as plt import numpy as np -import scipy.misc as sm -from autolab_core import BinaryClassificationResult, Point, TensorDataset, Logger -from autolab_core.constants import * +from autolab_core import BinaryClassificationResult, TensorDataset, Logger +from autolab_core.constants import JSON_INDENT import autolab_core.utils as utils from perception import DepthImage from visualization import Visualizer2D as vis2d from ..model import get_gqcnn_model from ..grasping import Grasp2D, SuctionPoint2D -from ..utils import GripperMode, ImageMode, GeneralConstants, read_pose_data, GQCNNFilenames +from ..utils import (GripperMode, GeneralConstants, read_pose_data, + GQCNNFilenames) -WINDOW = 100 # For loss. +WINDOW = 100 # For loss. MAX_LOSS = 5.0 + class GQCNNAnalyzer(object): """Analyzes a trained GQ-CNN model.""" @@ -74,7 +72,11 @@ def __init__(self, config, verbose=True, plot_backend="pdf"): self.cfg = config self.verbose = verbose - plt.switch_backend(plot_backend) # By default we want to use a non-interactive backend (ex. "pdf)" because the `GQCNNAnalyzer` anyways only saves plots to disk, and interactive backends sometimes cause issues with `localhost` when run remotely through the Linux `screen` program. + # By default we want to use a non-interactive backend (ex. "pdf)" + # because the `GQCNNAnalyzer` anyways only saves plots to disk, and + # interactive backends sometimes cause issues with `localhost` when run + # remotely through the Linux `screen` program. + plt.switch_backend(plot_backend) self._parse_config() def _parse_config(self): @@ -86,10 +88,8 @@ def _parse_config(self): self.dpi = self.cfg["dpi"] self.num_bins = self.cfg["num_bins"] self.num_vis = self.cfg["num_vis"] - - def analyze(self, model_dir, - output_dir, - dataset_config=None): + + def analyze(self, model_dir, output_dir, dataset_config=None): """Run analysis. Parameters @@ -120,27 +120,37 @@ def analyze(self, model_dir, os.mkdir(model_output_dir) # Set up logger. - self.logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(model_output_dir, "analysis.log"), silence=(not self.verbose), global_log_file=self.verbose) + self.logger = Logger.get_logger(self.__class__.__name__, + log_file=os.path.join( + model_output_dir, "analysis.log"), + silence=(not self.verbose), + global_log_file=self.verbose) + + self.logger.info("Analyzing model %s" % (model_name)) + self.logger.info("Saving output to %s" % (output_dir)) - self.logger.info("Analyzing model %s" %(model_name)) - self.logger.info("Saving output to %s" %(output_dir)) - # Run predictions. - train_result, val_result = self._run_prediction_single_model(model_dir, - model_output_dir, - dataset_config) + train_result, val_result = self._run_prediction_single_model( + model_dir, model_output_dir, dataset_config) # Finally plot curves. - init_train_error, final_train_error, init_train_loss, final_train_loss, init_val_error, final_val_error, norm_final_val_error = self._plot(model_dir, - model_output_dir, - train_result, - val_result) - - return train_result, val_result, init_train_error, final_train_error, init_train_loss, final_train_loss, init_val_error, final_val_error, norm_final_val_error - - def _plot_grasp(self, datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=None): + (init_train_error, final_train_error, init_train_loss, + final_train_loss, init_val_error, final_val_error, + norm_final_val_error) = self._plot(model_dir, model_output_dir, + train_result, val_result) + + return (train_result, val_result, init_train_error, final_train_error, + init_train_loss, final_train_loss, init_val_error, + final_val_error, norm_final_val_error) + + def _plot_grasp(self, + datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=None): """Plots a single grasp represented as a datapoint.""" - image = DepthImage(datapoint[image_field_name][:,:,0]) + image = DepthImage(datapoint[image_field_name][:, :, 0]) depth = datapoint[pose_field_name][2] width = 0 grasps = [] @@ -151,43 +161,56 @@ def _plot_grasp(self, datapoint, image_field_name, pose_field_name, gripper_mode bin_width = GeneralConstants.PI / num_bins for i in range(num_bins): bin_cent_ang = i * bin_width + bin_width / 2 - grasps.append(Grasp2D(center=image.center, angle=GeneralConstants.PI / 2 - bin_cent_ang, depth=depth, width=0.0)) - grasps.append(Grasp2D(center=image.center, angle=datapoint[pose_field_name][3], depth=depth, width=0.0)) + grasps.append( + Grasp2D(center=image.center, + angle=GeneralConstants.PI / 2 - bin_cent_ang, + depth=depth, + width=0.0)) + grasps.append( + Grasp2D(center=image.center, + angle=datapoint[pose_field_name][3], + depth=depth, + width=0.0)) else: - grasps.append(Grasp2D(center=image.center, + grasps.append( + Grasp2D(center=image.center, angle=0, depth=depth, width=0.0)) width = datapoint[pose_field_name][-1] else: - grasps.append(SuctionPoint2D(center=image.center, - axis=[1,0,0], - depth=depth)) + grasps.append( + SuctionPoint2D(center=image.center, + axis=[1, 0, 0], + depth=depth)) vis2d.imshow(image) for i, grasp in enumerate(grasps[:-1]): - vis2d.grasp(grasp, width=width, color=plt.cm.RdYlGn(angular_preds[i * 2 + 1])) + vis2d.grasp(grasp, + width=width, + color=plt.cm.RdYlGn(angular_preds[i * 2 + 1])) vis2d.grasp(grasps[-1], width=width, color="b") - - def _run_prediction_single_model(self, model_dir, - model_output_dir, + + def _run_prediction_single_model(self, model_dir, model_output_dir, dataset_config): """Analyze the performance of a single model.""" # Read in model config. - model_config_filename = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) + model_config_filename = os.path.join(model_dir, + GQCNNFilenames.SAVED_CFG) with open(model_config_filename) as data_file: model_config = json.load(data_file) # Load model. - self.logger.info("Loading model %s" %(model_dir)) + self.logger.info("Loading model %s" % (model_dir)) log_file = None for handler in self.logger.handlers: if isinstance(handler, logging.FileHandler): log_file = handler.baseFilename - gqcnn = get_gqcnn_model(verbose=self.verbose).load(model_dir, verbose=self.verbose, log_file=log_file) + gqcnn = get_gqcnn_model(verbose=self.verbose).load( + model_dir, verbose=self.verbose, log_file=log_file) gqcnn.open_session() gripper_mode = gqcnn.gripper_mode angular_bins = gqcnn.angular_bins - + # Read params from the config. if dataset_config is None: dataset_dir = model_config["dataset_dir"] @@ -204,24 +227,25 @@ def _run_prediction_single_model(self, model_dir, metric_name = dataset_config["target_metric_name"] metric_thresh = dataset_config["metric_thresh"] gripper_mode = dataset_config["gripper_mode"] - - self.logger.info("Loading dataset %s" %(dataset_dir)) + + self.logger.info("Loading dataset %s" % (dataset_dir)) dataset = TensorDataset.open(dataset_dir) train_indices, val_indices, _ = dataset.split(split_name) - + # Visualize conv filters. conv1_filters = gqcnn.filters num_filt = conv1_filters.shape[3] d = utils.sqrt_ceil(num_filt) vis2d.clf() for k in range(num_filt): - filt = conv1_filters[:,:,0,k] - vis2d.subplot(d,d,k+1) + filt = conv1_filters[:, :, 0, k] + vis2d.subplot(d, d, k + 1) vis2d.imshow(DepthImage(filt)) figname = os.path.join(model_output_dir, "conv1_filters.pdf") vis2d.savefig(figname, dpi=self.dpi) - - # Aggregate training and validation true labels and predicted probabilities. + + # Aggregate training and validation true labels and predicted + # probabilities. all_predictions = [] if angular_bins > 0: all_predictions_raw = [] @@ -229,49 +253,53 @@ def _run_prediction_single_model(self, model_dir, for i in range(dataset.num_tensors): # Log progress. if i % self.log_rate == 0: - self.logger.info("Predicting tensor %d of %d" %(i+1, dataset.num_tensors)) + self.logger.info("Predicting tensor %d of %d" % + (i + 1, dataset.num_tensors)) # Read in data. image_arr = dataset.tensor(image_field_name, i).arr - pose_arr = read_pose_data(dataset.tensor(pose_field_name, i).arr, - gripper_mode) + pose_arr = read_pose_data( + dataset.tensor(pose_field_name, i).arr, gripper_mode) metric_arr = dataset.tensor(metric_name, i).arr label_arr = 1 * (metric_arr > metric_thresh) label_arr = label_arr.astype(np.uint8) if angular_bins > 0: - # Form mask to extract predictions from ground-truth angular bins, + # Form mask to extract predictions from ground-truth angular + # bins. raw_poses = dataset.tensor(pose_field_name, i).arr angles = raw_poses[:, 3] neg_ind = np.where(angles < 0) - angles = np.abs(angles) % GeneralConstants.PI # TODO(vsatish): These should use the max angle instead. + # TODO(vsatish): These should use the max angle instead. + angles = np.abs(angles) % GeneralConstants.PI angles[neg_ind] *= -1 g_90 = np.where(angles > (GeneralConstants.PI / 2)) l_neg_90 = np.where(angles < (-1 * (GeneralConstants.PI / 2))) angles[g_90] -= GeneralConstants.PI angles[l_neg_90] += GeneralConstants.PI - angles *= -1 # Hack to fix reverse angle convention. - # TODO(vsatish): Fix this along with the others. + # TODO(vsatish): Fix this along with the others. + angles *= -1 # Hack to fix reverse angle convention. angles += (GeneralConstants.PI / 2) - pred_mask = np.zeros((raw_poses.shape[0], angular_bins*2), dtype=bool) + pred_mask = np.zeros((raw_poses.shape[0], angular_bins * 2), + dtype=bool) bin_width = GeneralConstants.PI / angular_bins for i in range(angles.shape[0]): - pred_mask[i, int((angles[i] // bin_width)*2)] = True - pred_mask[i, int((angles[i] // bin_width)*2 + 1)] = True + pred_mask[i, int((angles[i] // bin_width) * 2)] = True + pred_mask[i, int((angles[i] // bin_width) * 2 + 1)] = True # Predict with GQ-CNN. predictions = gqcnn.predict(image_arr, pose_arr) if angular_bins > 0: raw_predictions = np.array(predictions) predictions = predictions[pred_mask].reshape((-1, 2)) - + # Aggregate. - all_predictions.extend(predictions[:,1].tolist()) + all_predictions.extend(predictions[:, 1].tolist()) if angular_bins > 0: all_predictions_raw.extend(raw_predictions.tolist()) all_labels.extend(label_arr.tolist()) - + # Close session. - gqcnn.close_session() + gqcnn.close_session() # Create arrays. all_predictions = np.array(all_predictions) @@ -283,20 +311,25 @@ def _run_prediction_single_model(self, model_dir, if angular_bins > 0: all_predictions_raw = np.array(all_predictions_raw) train_predictions_raw = all_predictions_raw[train_indices] - val_predictions_raw = all_predictions_raw[val_indices] + val_predictions_raw = all_predictions_raw[val_indices] # Aggregate results. - train_result = BinaryClassificationResult(train_predictions, train_labels) + train_result = BinaryClassificationResult(train_predictions, + train_labels) val_result = BinaryClassificationResult(val_predictions, val_labels) train_result.save(os.path.join(model_output_dir, "train_result.cres")) val_result.save(os.path.join(model_output_dir, "val_result.cres")) # Get stats, plot curves. - self.logger.info("Model %s training error rate: %.3f" %(model_dir, train_result.error_rate)) - self.logger.info("Model %s validation error rate: %.3f" %(model_dir, val_result.error_rate)) + self.logger.info("Model %s training error rate: %.3f" % + (model_dir, train_result.error_rate)) + self.logger.info("Model %s validation error rate: %.3f" % + (model_dir, val_result.error_rate)) - self.logger.info("Model %s training loss: %.3f" %(model_dir, train_result.cross_entropy_loss)) - self.logger.info("Model %s validation loss: %.3f" %(model_dir, val_result.cross_entropy_loss)) + self.logger.info("Model %s training loss: %.3f" % + (model_dir, train_result.cross_entropy_loss)) + self.logger.info("Model %s validation loss: %.3f" % + (model_dir, val_result.cross_entropy_loss)) # Save images. vis2d.figure() @@ -309,25 +342,32 @@ def _run_prediction_single_model(self, model_dir, train_example_dir = os.path.join(example_dir, "train") if not os.path.exists(train_example_dir): os.mkdir(train_example_dir) - + # Train TP. true_positive_indices = train_result.true_positive_indices np.random.shuffle(true_positive_indices) true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = train_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=train_predictions_raw[j]) else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - train_result.pred_probs[j], - train_result.labels[j]), - fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, "true_positive_%03d.png" %(i))) + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title( + "Datapoint %d: Pred: %.3f Label: %.3f" % + (k, train_result.pred_probs[j], train_result.labels[j]), + fontsize=self.font_size) + vis2d.savefig( + os.path.join(train_example_dir, + "true_positive_%03d.png" % (i))) # Train FP. false_positive_indices = train_result.false_positive_indices @@ -335,18 +375,25 @@ def _run_prediction_single_model(self, model_dir, false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = train_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - train_result.pred_probs[j], - train_result.labels[j]), - fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, "false_positive_%03d.png" %(i))) + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=train_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title( + "Datapoint %d: Pred: %.3f Label: %.3f" % + (k, train_result.pred_probs[j], train_result.labels[j]), + fontsize=self.font_size) + vis2d.savefig( + os.path.join(train_example_dir, + "false_positive_%03d.png" % (i))) # Train TN. true_negative_indices = train_result.true_negative_indices @@ -354,18 +401,25 @@ def _run_prediction_single_model(self, model_dir, true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = train_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - train_result.pred_probs[j], - train_result.labels[j]), - fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, "true_negative_%03d.png" %(i))) + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=train_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title( + "Datapoint %d: Pred: %.3f Label: %.3f" % + (k, train_result.pred_probs[j], train_result.labels[j]), + fontsize=self.font_size) + vis2d.savefig( + os.path.join(train_example_dir, + "true_negative_%03d.png" % (i))) # Train TP. false_negative_indices = train_result.false_negative_indices @@ -373,18 +427,25 @@ def _run_prediction_single_model(self, model_dir, false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = train_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=train_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - train_result.pred_probs[j], - train_result.labels[j]), - fontsize=self.font_size) - vis2d.savefig(os.path.join(train_example_dir, "false_negative_%03d.png" %(i))) + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=train_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title( + "Datapoint %d: Pred: %.3f Label: %.3f" % + (k, train_result.pred_probs[j], train_result.labels[j]), + fontsize=self.font_size) + vis2d.savefig( + os.path.join(train_example_dir, + "false_negative_%03d.png" % (i))) # Val. self.logger.info("Saving validation examples") @@ -398,18 +459,23 @@ def _run_prediction_single_model(self, model_dir, true_positive_indices = true_positive_indices[:self.num_vis] for i, j in enumerate(true_positive_indices): k = val_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - val_result.pred_probs[j], - val_result.labels[j]), + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=val_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % + (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, "true_positive_%03d.png" %(i))) + vis2d.savefig( + os.path.join(val_example_dir, "true_positive_%03d.png" % (i))) # Val FP. false_positive_indices = val_result.false_positive_indices @@ -417,18 +483,23 @@ def _run_prediction_single_model(self, model_dir, false_positive_indices = false_positive_indices[:self.num_vis] for i, j in enumerate(false_positive_indices): k = val_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - val_result.pred_probs[j], - val_result.labels[j]), + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=val_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % + (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, "false_positive_%03d.png" %(i))) + vis2d.savefig( + os.path.join(val_example_dir, "false_positive_%03d.png" % (i))) # Val TN. true_negative_indices = val_result.true_negative_indices @@ -436,18 +507,23 @@ def _run_prediction_single_model(self, model_dir, true_negative_indices = true_negative_indices[:self.num_vis] for i, j in enumerate(true_negative_indices): k = val_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - val_result.pred_probs[j], - val_result.labels[j]), + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=val_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % + (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, "true_negative_%03d.png" %(i))) + vis2d.savefig( + os.path.join(val_example_dir, "true_negative_%03d.png" % (i))) # Val TP. false_negative_indices = val_result.false_negative_indices @@ -455,19 +531,24 @@ def _run_prediction_single_model(self, model_dir, false_negative_indices = false_negative_indices[:self.num_vis] for i, j in enumerate(false_negative_indices): k = val_indices[j] - datapoint = dataset.datapoint(k, field_names=[image_field_name, - pose_field_name]) + datapoint = dataset.datapoint( + k, field_names=[image_field_name, pose_field_name]) vis2d.clf() if angular_bins > 0: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode, angular_preds=val_predictions_raw[j]) - else: - self._plot_grasp(datapoint, image_field_name, pose_field_name, gripper_mode) - vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" %(k, - val_result.pred_probs[j], - val_result.labels[j]), + self._plot_grasp(datapoint, + image_field_name, + pose_field_name, + gripper_mode, + angular_preds=val_predictions_raw[j]) + else: + self._plot_grasp(datapoint, image_field_name, pose_field_name, + gripper_mode) + vis2d.title("Datapoint %d: Pred: %.3f Label: %.3f" % + (k, val_result.pred_probs[j], val_result.labels[j]), fontsize=self.font_size) - vis2d.savefig(os.path.join(val_example_dir, "false_negative_%03d.png" %(i))) - + vis2d.savefig( + os.path.join(val_example_dir, "false_negative_%03d.png" % (i))) + # Save summary stats. train_summary_stats = { "error_rate": train_result.error_rate, @@ -475,8 +556,10 @@ def _run_prediction_single_model(self, model_dir, "auc_score": train_result.auc_score, "loss": train_result.cross_entropy_loss } - train_stats_filename = os.path.join(model_output_dir, "train_stats.json") - json.dump(train_summary_stats, open(train_stats_filename, "w"), + train_stats_filename = os.path.join(model_output_dir, + "train_stats.json") + json.dump(train_summary_stats, + open(train_stats_filename, "w"), indent=JSON_INDENT, sort_keys=True) @@ -484,13 +567,14 @@ def _run_prediction_single_model(self, model_dir, "error_rate": val_result.error_rate, "ap_score": val_result.ap_score, "auc_score": val_result.auc_score, - "loss": val_result.cross_entropy_loss + "loss": val_result.cross_entropy_loss } val_stats_filename = os.path.join(model_output_dir, "val_stats.json") - json.dump(val_summary_stats, open(val_stats_filename, "w"), + json.dump(val_summary_stats, + open(val_stats_filename, "w"), indent=JSON_INDENT, - sort_keys=True) - + sort_keys=True) + return train_result, val_result def _plot(self, model_dir, model_output_dir, train_result, val_result): @@ -498,12 +582,10 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): self.logger.info("Plotting") _, model_name = os.path.split(model_output_dir) - + # Set params. colors = ["g", "b", "c", "y", "m", "r"] - styles = ["-", "--", "-.", ":", "-"] - num_colors = len(colors) - num_styles = len(styles) + styles = ["-", "--", "-.", ":", "-"] # PR, ROC. vis2d.clf() @@ -534,43 +616,50 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): color=colors[1], style=styles[1], label="VAL") - vis2d.title("Reciever Operating Characteristic", fontsize=self.font_size) + vis2d.title("Reciever Operating Characteristic", + fontsize=self.font_size) handles, labels = vis2d.gca().get_legend_handles_labels() vis2d.legend(handles, labels, loc="best") figname = os.path.join(model_output_dir, "roc.png") vis2d.savefig(figname, dpi=self.dpi) - + # Plot histogram of prediction errors. num_bins = min(self.num_bins, train_result.num_datapoints) - + # Train positives. pos_ind = np.where(train_result.labels == 1)[0] - diffs = np.abs(train_result.labels[pos_ind] - train_result.pred_probs[pos_ind]) + diffs = np.abs(train_result.labels[pos_ind] - + train_result.pred_probs[pos_ind]) vis2d.figure() utils.histogram(diffs, num_bins, - bounds=(0,1), + bounds=(0, 1), normalized=False, plot=True) - vis2d.title("Error on Positive Training Examples", fontsize=self.font_size) + vis2d.title("Error on Positive Training Examples", + fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) - figname = os.path.join(model_output_dir, "pos_train_errors_histogram.png") + figname = os.path.join(model_output_dir, + "pos_train_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Train negatives. neg_ind = np.where(train_result.labels == 0)[0] - diffs = np.abs(train_result.labels[neg_ind] - train_result.pred_probs[neg_ind]) + diffs = np.abs(train_result.labels[neg_ind] - + train_result.pred_probs[neg_ind]) vis2d.figure() utils.histogram(diffs, num_bins, - bounds=(0,1), + bounds=(0, 1), normalized=False, plot=True) - vis2d.title("Error on Negative Training Examples", fontsize=self.font_size) + vis2d.title("Error on Negative Training Examples", + fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) - figname = os.path.join(model_output_dir, "neg_train_errors_histogram.png") + figname = os.path.join(model_output_dir, + "neg_train_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Histogram of validation errors. @@ -578,46 +667,55 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): # Val positives. pos_ind = np.where(val_result.labels == 1)[0] - diffs = np.abs(val_result.labels[pos_ind] - val_result.pred_probs[pos_ind]) + diffs = np.abs(val_result.labels[pos_ind] - + val_result.pred_probs[pos_ind]) vis2d.figure() utils.histogram(diffs, num_bins, - bounds=(0,1), + bounds=(0, 1), normalized=False, plot=True) - vis2d.title("Error on Positive Validation Examples", fontsize=self.font_size) + vis2d.title("Error on Positive Validation Examples", + fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) - figname = os.path.join(model_output_dir, "pos_val_errors_histogram.png") + figname = os.path.join(model_output_dir, + "pos_val_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Val negatives. neg_ind = np.where(val_result.labels == 0)[0] - diffs = np.abs(val_result.labels[neg_ind] - val_result.pred_probs[neg_ind]) + diffs = np.abs(val_result.labels[neg_ind] - + val_result.pred_probs[neg_ind]) vis2d.figure() utils.histogram(diffs, num_bins, - bounds=(0,1), + bounds=(0, 1), normalized=False, plot=True) - vis2d.title("Error on Negative Validation Examples", fontsize=self.font_size) + vis2d.title("Error on Negative Validation Examples", + fontsize=self.font_size) vis2d.xlabel("Abs Prediction Error", fontsize=self.font_size) vis2d.ylabel("Count", fontsize=self.font_size) - figname = os.path.join(model_output_dir, "neg_val_errors_histogram.png") + figname = os.path.join(model_output_dir, + "neg_val_errors_histogram.png") vis2d.savefig(figname, dpi=self.dpi) # Losses. try: - train_errors_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_ERRORS) - val_errors_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ERRORS) - train_iters_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_ITERS) - val_iters_filename = os.path.join(model_dir, GQCNNFilenames.VAL_ITERS) - pct_pos_val_filename = os.path.join(model_dir, GQCNNFilenames.PCT_POS_VAL) - train_losses_filename = os.path.join(model_dir, GQCNNFilenames.TRAIN_LOSS) + train_errors_filename = os.path.join(model_dir, + GQCNNFilenames.TRAIN_ERRORS) + val_errors_filename = os.path.join(model_dir, + GQCNNFilenames.VAL_ERRORS) + val_iters_filename = os.path.join(model_dir, + GQCNNFilenames.VAL_ITERS) + pct_pos_val_filename = os.path.join(model_dir, + GQCNNFilenames.PCT_POS_VAL) + train_losses_filename = os.path.join(model_dir, + GQCNNFilenames.TRAIN_LOSS) raw_train_errors = np.load(train_errors_filename) val_errors = np.load(val_errors_filename) - raw_train_iters = np.load(train_iters_filename) val_iters = np.load(val_iters_filename) pct_pos_val = float(val_errors[0]) if os.path.exists(pct_pos_val_filename): @@ -626,72 +724,91 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): val_errors = np.r_[pct_pos_val, val_errors] val_iters = np.r_[0, val_iters] - + # Window the training error. i = 0 train_errors = [] train_losses = [] train_iters = [] while i < raw_train_errors.shape[0]: - train_errors.append(np.mean(raw_train_errors[i:i+WINDOW])) - train_losses.append(np.mean(raw_train_losses[i:i+WINDOW])) + train_errors.append(np.mean(raw_train_errors[i:i + WINDOW])) + train_losses.append(np.mean(raw_train_losses[i:i + WINDOW])) train_iters.append(i) i += WINDOW train_errors = np.array(train_errors) train_losses = np.array(train_losses) train_iters = np.array(train_iters) - + init_val_error = val_errors[0] norm_train_errors = train_errors / init_val_error norm_val_errors = val_errors / init_val_error norm_final_val_error = val_result.error_rate / val_errors[0] if pct_pos_val > 0: - norm_final_val_error = val_result.error_rate / pct_pos_val - + norm_final_val_error = val_result.error_rate / pct_pos_val + vis2d.clf() - vis2d.plot(train_iters, train_errors, linewidth=self.line_width, color="b") - vis2d.plot(val_iters, val_errors, linewidth=self.line_width, color="g") + vis2d.plot(train_iters, + train_errors, + linewidth=self.line_width, + color="b") + vis2d.plot(val_iters, + val_errors, + linewidth=self.line_width, + color="g") vis2d.ylim(0, 100) - vis2d.legend(("TRAIN (Minibatch)", "VAL"), fontsize=self.font_size, loc="best") + vis2d.legend(("TRAIN (Minibatch)", "VAL"), + fontsize=self.font_size, + loc="best") vis2d.xlabel("Iteration", fontsize=self.font_size) vis2d.ylabel("Error Rate", fontsize=self.font_size) - vis2d.title("Error Rate vs Training Iteration", fontsize=self.font_size) - figname = os.path.join(model_output_dir, "training_error_rates.png") + vis2d.title("Error Rate vs Training Iteration", + fontsize=self.font_size) + figname = os.path.join(model_output_dir, + "training_error_rates.png") vis2d.savefig(figname, dpi=self.dpi) - + vis2d.clf() vis2d.plot(train_iters, norm_train_errors, linewidth=4, color="b") vis2d.plot(val_iters, norm_val_errors, linewidth=4, color="g") vis2d.ylim(0, 2.0) - vis2d.legend(("TRAIN (Minibatch)", "VAL"), fontsize=self.font_size, loc="best") + vis2d.legend(("TRAIN (Minibatch)", "VAL"), + fontsize=self.font_size, + loc="best") vis2d.xlabel("Iteration", fontsize=self.font_size) vis2d.ylabel("Normalized Error Rate", fontsize=self.font_size) - vis2d.title("Normalized Error Rate vs Training Iteration", fontsize=self.font_size) - figname = os.path.join(model_output_dir, "training_norm_error_rates.png") + vis2d.title("Normalized Error Rate vs Training Iteration", + fontsize=self.font_size) + figname = os.path.join(model_output_dir, + "training_norm_error_rates.png") vis2d.savefig(figname, dpi=self.dpi) - train_losses[train_losses > MAX_LOSS] = MAX_LOSS # CAP LOSSES. + train_losses[train_losses > MAX_LOSS] = MAX_LOSS # CAP LOSSES. vis2d.clf() - vis2d.plot(train_iters, train_losses, linewidth=self.line_width, color="b") + vis2d.plot(train_iters, + train_losses, + linewidth=self.line_width, + color="b") vis2d.ylim(0, 2.0) vis2d.xlabel("Iteration", fontsize=self.font_size) vis2d.ylabel("Loss", fontsize=self.font_size) vis2d.title("Training Loss vs Iteration", fontsize=self.font_size) figname = os.path.join(model_output_dir, "training_losses.png") vis2d.savefig(figname, dpi=self.dpi) - + # Log. self.logger.info("TRAIN") - self.logger.info("Original error: %.3f" %(train_errors[0])) - self.logger.info("Final error: %.3f" %(train_result.error_rate)) - self.logger.info("Orig loss: %.3f" %(train_losses[0])) - self.logger.info("Final loss: %.3f" %(train_losses[-1])) - + self.logger.info("Original error: %.3f" % (train_errors[0])) + self.logger.info("Final error: %.3f" % (train_result.error_rate)) + self.logger.info("Orig loss: %.3f" % (train_losses[0])) + self.logger.info("Final loss: %.3f" % (train_losses[-1])) + self.logger.info("VAL") - self.logger.info("Original error: %.3f" %(pct_pos_val)) - self.logger.info("Final error: %.3f" %(val_result.error_rate)) - self.logger.info("Normalized error: %.3f" %(norm_final_val_error)) + self.logger.info("Original error: %.3f" % (pct_pos_val)) + self.logger.info("Final error: %.3f" % (val_result.error_rate)) + self.logger.info("Normalized error: %.3f" % (norm_final_val_error)) - return train_errors[0], train_result.error_rate, train_losses[0], train_losses[-1], pct_pos_val, val_result.error_rate, norm_final_val_error + return (train_errors[0], train_result.error_rate, train_losses[0], + train_losses[-1], pct_pos_val, val_result.error_rate, + norm_final_val_error) except Exception as e: self.logger.error("Failed to plot training curves!\n" + str(e)) diff --git a/gqcnn/grasping/__init__.py b/gqcnn/grasping/__init__.py index 6ad8c4af..7151f43d 100644 --- a/gqcnn/grasping/__init__.py +++ b/gqcnn/grasping/__init__.py @@ -27,19 +27,25 @@ from __future__ import print_function from .grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D -from .grasp_quality_function import (GraspQualityFunctionFactory, +from .grasp_quality_function import (GraspQualityFunctionFactory, GQCnnQualityFunction) from .image_grasp_sampler import (ImageGraspSamplerFactory, AntipodalDepthImageGraspSampler) -from .constraint_fn import (GraspConstraintFnFactory, - DiscreteApproachGraspConstraintFn) +from .constraint_fn import GraspConstraintFnFactory from .policy import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction, - UniformRandomGraspingPolicy, - PriorityCompositeGraspingPolicy, - RgbdImageState, GraspAction) + UniformRandomGraspingPolicy, RgbdImageState, GraspAction) from .actions import (NoAction, ParallelJawGrasp3D, SuctionGrasp3D, MultiSuctionGrasp3D) -__all__ = ["Grasp2D", "SuctionPoint2D", "MultiSuctionPoint2D", "GraspQualityFunctionFactory", "GQCnnQualityFunction", "ImageGraspSamplerFactory", "AntipodalDepthImageGraspSampler", "RobustGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "UniformRandomGraspingPolicy", "RgbdImageState", "GraspAction", "GraspConstraintFnFactory", "NoAction", "ParallelJawGrasp3D", "SuctionGrasp3D", "MultiSuctionGrasp3D"] +__all__ = [ + "Grasp2D", "SuctionPoint2D", "MultiSuctionPoint2D", + "GraspQualityFunctionFactory", "GQCnnQualityFunction", + "ImageGraspSamplerFactory", "AntipodalDepthImageGraspSampler", + "RobustGraspingPolicy", "CrossEntropyRobustGraspingPolicy", + "FullyConvolutionalGraspingPolicyParallelJaw", + "FullyConvolutionalGraspingPolicySuction", "UniformRandomGraspingPolicy", + "RgbdImageState", "GraspAction", "GraspConstraintFnFactory", "NoAction", + "ParallelJawGrasp3D", "SuctionGrasp3D", "MultiSuctionGrasp3D" +] diff --git a/gqcnn/grasping/actions.py b/gqcnn/grasping/actions.py index 9578582f..dd43511a 100644 --- a/gqcnn/grasping/actions.py +++ b/gqcnn/grasping/actions.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Action classes for representing 3D grasp actions. -Author: Jeff Mahler + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -34,25 +37,25 @@ from future.utils import with_metaclass import numpy as np -from autolab_core import Point, RigidTransform +from autolab_core import Point from .grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D + class Action(with_metaclass(ABCMeta, object)): """Abstract action class. Attributes ---------- q_value : float - q_value of the grasp + Grasp quality. id : int - integer identifier for the action + Integer identifier for the action. metadata : dict - key-value dict of extra data about the action + Key-value dict of extra data about the action. """ - def __init__(self, q_value=0.0, - id=-1, - metadata={}): + + def __init__(self, q_value=0.0, id=-1, metadata={}): self._q_value = q_value self._id = id self._metadata = metadata @@ -64,46 +67,45 @@ def q_value(self): @property def id(self): return self._id - + @property def metadata(self): return self._metadata + class NoAction(Action): """Proxy for taking no action when none can be found.""" pass - + + class GraspAction3D(with_metaclass(ABCMeta, Action)): """Abstract grasp class with grasp specified as an end-effector pose. Attributes ---------- T_grasp_world : :obj:`RigidTransform` - pose of the grasp w.r.t. world coordinate frame + Pose of the grasp w.r.t. world coordinate frame. """ - def __init__(self, T_grasp_world, - q_value=0.0, - id=-1, - metadata={}): + + def __init__(self, T_grasp_world, q_value=0.0, id=-1, metadata={}): self.T_grasp_world = T_grasp_world Action.__init__(self, q_value, id, metadata) @abstractmethod - def project(self, camera_intr, - T_camera_world): + def project(self, camera_intr, T_camera_world): pass + class ParallelJawGrasp3D(GraspAction3D): """Grasping with a parallel-jaw gripper. Attributes ---------- T_grasp_world : :obj:`RigidTransform` - pose of the grasp wrt world coordinate frame - """ - def project(self, camera_intr, - T_camera_world, - gripper_width=0.05): + Pose of the grasp wrt world coordinate frame. + """ + + def project(self, camera_intr, T_camera_world, gripper_width=0.05): # Compute pose of grasp in camera frame. T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world y_axis_camera = T_grasp_camera.y_axis[:2] @@ -121,8 +123,7 @@ def project(self, camera_intr, # Compute grasp center in image space. t_grasp_camera = T_grasp_camera.translation - p_grasp_camera = Point(t_grasp_camera, - frame=camera_intr.frame) + p_grasp_camera = Point(t_grasp_camera, frame=camera_intr.frame) u_grasp_camera = camera_intr.project(p_grasp_camera) d_grasp_camera = t_grasp_camera[2] return Grasp2D(u_grasp_camera, @@ -130,17 +131,18 @@ def project(self, camera_intr, d_grasp_camera, width=gripper_width, camera_intr=camera_intr) - + + class SuctionGrasp3D(GraspAction3D): """Grasping with a suction-based gripper. Attributes ---------- T_grasp_world : :obj:`RigidTransform` - pose of the grasp wrt world coordinate frame + Pose of the grasp wrt world coordinate frame. """ - def project(self, camera_intr, - T_camera_world): + + def project(self, camera_intr, T_camera_world): # Compute pose of grasp in camera frame. T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world x_axis_camera = T_grasp_camera.x_axis @@ -155,17 +157,17 @@ def project(self, camera_intr, d_grasp_camera, camera_intr=camera_intr) + class MultiSuctionGrasp3D(GraspAction3D): """Grasping with a multi-cup suction-based gripper. Attributes ---------- T_grasp_world : :obj:`RigidTransform` - pose of the grasp wrt world coordinate frame + Pose of the grasp wrt world coordinate frame. """ - def project(self, camera_intr, - T_camera_world): + + def project(self, camera_intr, T_camera_world): # Compute pose of grasp in camera frame. T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world - return MultiSuctionPoint2D(T_grasp_camera, - camera_intr=camera_intr) + return MultiSuctionPoint2D(T_grasp_camera, camera_intr=camera_intr) diff --git a/gqcnn/grasping/constraint_fn.py b/gqcnn/grasping/constraint_fn.py index a1e4df46..670e52be 100644 --- a/gqcnn/grasping/constraint_fn.py +++ b/gqcnn/grasping/constraint_fn.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Constraint functions for grasp sampling. -Author: Jeff Mahler + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -34,70 +37,72 @@ from future.utils import with_metaclass import numpy as np + class GraspConstraintFn(with_metaclass(ABCMeta, object)): """Abstract constraint functions for grasp sampling.""" def __init__(self, config): # Set params. self._config = config - + def __call__(self, grasp): """Evaluates whether or not a grasp is valid. Parameters ---------- grasp : :obj:`Grasp2D` - grasp to evaluate + Grasp to evaluate. Returns ------- bool - True if the grasp satisfies constraints, False otherwise + True if the grasp satisfies constraints, False otherwise. """ return self.satisfies_constraints(grasp) - @abstractmethod + @abstractmethod def satisfies_constraints(self, grasp): """Evaluates whether or not a grasp is valid. Parameters ---------- grasp : :obj:`Grasp2D` - grasp to evaluate + Grasp to evaluate. Returns ------- bool - True if the grasp satisfies constraints, False otherwise + True if the grasp satisfies constraints, False otherwise. """ pass + class DiscreteApproachGraspConstraintFn(GraspConstraintFn): """Constrains the grasp approach direction into a discrete set of - angles from the world z direction. - """ + angles from the world z direction.""" + def __init__(self, config): # Init superclass. GraspConstraintFn.__init__(self, config) - + self._max_approach_angle = self._config["max_approach_angle"] self._angular_tolerance = self._config["angular_tolerance"] self._angular_step = self._config["angular_step"] self._T_camera_world = self._config["camera_pose"] - + def satisfies_constraints(self, grasp): """Evaluates whether or not a grasp is valid by evaluating the - angle between the approach axis and the world z direction. + angle between the approach axis and the world z direction. Parameters ---------- grasp : :obj:`Grasp2D` - grasp to evaluate + Grasp to evaluate. Returns ------- bool - True if the grasp satisfies constraints, False otherwise + True if the grasp satisfies constraints, False otherwise. """ # Find grasp angle in world coordinates. axis_world = self._T_camera_world.rotation.dot(grasp.approach_axis) @@ -111,11 +116,11 @@ def satisfies_constraints(self, grasp): step=self._angular_step) diff = np.abs(available_angles - angle) angle_index = np.argmin(diff) - closest_angle = available_angles[angle_index] if diff[angle_index] < self._angular_tolerance: return True return False + class GraspConstraintFnFactory(object): @staticmethod def constraint_fn(fn_type, config): @@ -124,4 +129,6 @@ def constraint_fn(fn_type, config): elif fn_type == "discrete_approach_angle": return DiscreteApproachGraspConstraintFn(config) else: - raise ValueError("Grasp constraint function type {} not supported!".format(fn_type)) + raise ValueError( + "Grasp constraint function type {} not supported!".format( + fn_type)) diff --git a/gqcnn/grasping/grasp.py b/gqcnn/grasping/grasp.py index b13fa77d..254ecf61 100644 --- a/gqcnn/grasping/grasp.py +++ b/gqcnn/grasping/grasp.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Classes to encapsulate parallel-jaw grasps in image space. -Author: Jeff Mahler + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -34,6 +37,7 @@ from autolab_core import Point, RigidTransform from perception import CameraIntrinsics + class Grasp2D(object): """Parallel-jaw grasp in image space. @@ -54,16 +58,29 @@ class Grasp2D(object): contact_normals : list of :obj:`numpy.ndarray` Pair of contact normals in image space. """ - def __init__(self, center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, - contact_points=None, contact_normals=None): + + def __init__(self, + center, + angle=0.0, + depth=1.0, + width=0.0, + camera_intr=None, + contact_points=None, + contact_normals=None): self.center = center self.angle = angle self.depth = depth self.width = width # If `camera_intr` is none use default primesense camera intrinsics. if not camera_intr: - self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) - else: + self.camera_intr = CameraIntrinsics("primesense_overhead", + fx=525, + fy=525, + cx=319.5, + cy=239.5, + width=640, + height=480) + else: self.camera_intr = camera_intr self.contact_points = contact_points self.contact_normals = contact_normals @@ -73,22 +90,22 @@ def __init__(self, center, angle=0.0, depth=1.0, width=0.0, camera_intr=None, frame = camera_intr.frame if isinstance(center, np.ndarray): self.center = Point(center, frame=frame) - + @property def axis(self): """Returns the grasp axis.""" - return np.array([np.cos(self.angle), np.sin(self.angle)]) + return np.array([np.cos(self.angle), np.sin(self.angle)]) @property def approach_axis(self): - return np.array([0,0,1]) - + return np.array([0, 0, 1]) + @property def approach_angle(self): """The angle between the grasp approach axis and camera optical axis. """ return 0.0 - + @property def frame(self): """The name of the frame of reference for the grasp.""" @@ -101,11 +118,13 @@ def frame(self): def width_px(self): """Returns the width in pixels.""" if self.camera_intr is None: - raise ValueError("Must specify camera intrinsics to compute gripper width in 3D space") + missing_camera_intr_msg = ("Must specify camera intrinsics to" + " compute gripper width in 3D space.") + raise ValueError(missing_camera_intr_msg) # Form the jaw locations in 3D space at the given depth. p1 = Point(np.array([0, 0, self.depth]), frame=self.frame) p2 = Point(np.array([self.width, 0, self.depth]), frame=self.frame) - + # Project into pixel space. u1 = self.camera_intr.project(p1) u2 = self.camera_intr.project(p2) @@ -131,7 +150,7 @@ def feature_vec(self): @staticmethod def from_feature_vec(v, width=0.0, camera_intr=None): """Creates a `Grasp2D` instance from a feature vector and additional - parameters. + parameters. Parameters ---------- @@ -157,14 +176,18 @@ def from_feature_vec(v, width=0.0, camera_intr=None): angle = np.arccos(axis[0]) else: angle = -np.arccos(axis[0]) - return Grasp2D(center, angle, depth, width=width, camera_intr=camera_intr) + return Grasp2D(center, + angle, + depth, + width=width, + camera_intr=camera_intr) def pose(self, 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 : :obj:`numpy.ndarray` @@ -173,37 +196,41 @@ def pose(self, grasp_approach_dir=None): Returns ------- - :obj:`autolab_core.RigidTransform`: The transformation from the grasp - to the camera frame of reference. + :obj:`autolab_core.RigidTransform` + The transformation from the grasp to the camera frame of reference. """ # Check intrinsics. if self.camera_intr is None: - raise ValueError("Must specify camera intrinsics to compute 3D grasp pose") + raise ValueError( + "Must specify camera intrinsics to compute 3D grasp pose") # Compute 3D grasp center in camera basis. grasp_center_im = self.center.data center_px_im = Point(grasp_center_im, frame=self.camera_intr.frame) - grasp_center_camera = self.camera_intr.deproject_pixel(self.depth, center_px_im) + grasp_center_camera = self.camera_intr.deproject_pixel( + self.depth, center_px_im) grasp_center_camera = grasp_center_camera.data # Compute 3D grasp axis in camera basis. grasp_axis_im = self.axis grasp_axis_im = grasp_axis_im / np.linalg.norm(grasp_axis_im) grasp_axis_camera = np.array([grasp_axis_im[0], grasp_axis_im[1], 0]) - grasp_axis_camera = grasp_axis_camera / np.linalg.norm(grasp_axis_camera) - + grasp_axis_camera = grasp_axis_camera / np.linalg.norm( + grasp_axis_camera) + # Convert to 3D pose. - grasp_rot_camera, _, _ = np.linalg.svd(grasp_axis_camera.reshape(3,1)) + grasp_rot_camera, _, _ = np.linalg.svd(grasp_axis_camera.reshape(3, 1)) grasp_x_camera = grasp_approach_dir if grasp_approach_dir is None: - grasp_x_camera = np.array([0,0,1]) # Align with camera Z axis. + grasp_x_camera = np.array([0, 0, 1]) # Align with camera Z axis. grasp_y_camera = grasp_axis_camera grasp_z_camera = np.cross(grasp_x_camera, grasp_y_camera) grasp_z_camera = grasp_z_camera / np.linalg.norm(grasp_z_camera) grasp_y_camera = np.cross(grasp_z_camera, grasp_x_camera) - grasp_rot_camera = np.array([grasp_x_camera, grasp_y_camera, grasp_z_camera]).T - if np.linalg.det(grasp_rot_camera) < 0: # Fix possible reflections due to SVD. - grasp_rot_camera[:,0] = -grasp_rot_camera[:,0] + grasp_rot_camera = np.array( + [grasp_x_camera, grasp_y_camera, grasp_z_camera]).T + if np.linalg.det(grasp_rot_camera) < 0: # Fix reflections due to SVD. + grasp_rot_camera[:, 0] = -grasp_rot_camera[:, 0] T_grasp_camera = RigidTransform(rotation=grasp_rot_camera, translation=grasp_center_camera, from_frame="grasp", @@ -227,7 +254,8 @@ def image_dist(g1, g2, alpha=1.0): Returns ------- - float: Distance between grasps. + float + Distance between grasps. """ # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) @@ -237,6 +265,7 @@ def image_dist(g1, g2, alpha=1.0): axis_dist = np.arccos(dot) return point_dist + alpha * axis_dist + class SuctionPoint2D(object): """Suction grasp in image space. @@ -251,9 +280,10 @@ class SuctionPoint2D(object): camera_intr : :obj:`perception.CameraIntrinsics` Frame of reference for camera that the suction point corresponds to. """ + def __init__(self, center, axis=None, depth=1.0, camera_intr=None): if axis is None: - axis = np.array([0,0,1]) + axis = np.array([0, 0, 1]) self.center = center self.axis = axis @@ -271,8 +301,14 @@ def __init__(self, center, axis=None, depth=1.0, camera_intr=None): self.depth = depth # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: - self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) - else: + self.camera_intr = CameraIntrinsics("primesense_overhead", + fx=525, + fy=525, + cx=319.5, + cy=239.5, + width=640, + height=480) + else: self.camera_intr = camera_intr @property @@ -285,11 +321,12 @@ def frame(self): @property def angle(self): """The angle that the grasp pivot axis makes in image space.""" - rotation_axis = np.cross(self.axis, np.array([0,0,1])) + rotation_axis = np.cross(self.axis, np.array([0, 0, 1])) rotation_axis_image = np.array([rotation_axis[0], rotation_axis[1]]) angle = 0 if np.linalg.norm(rotation_axis) > 0: - rotation_axis_image = rotation_axis_image / np.linalg.norm(rotation_axis_image) + rotation_axis_image = rotation_axis_image / np.linalg.norm( + rotation_axis_image) angle = np.arccos(rotation_axis_image[0]) if rotation_axis[1] < 0: angle = -angle @@ -299,13 +336,13 @@ def angle(self): def approach_angle(self): """The angle between the grasp approach axis and camera optical axis. """ - dot = max(min(self.axis.dot(np.array([0,0,1])), 1.0), -1.0) + dot = max(min(self.axis.dot(np.array([0, 0, 1])), 1.0), -1.0) return np.arccos(dot) @property def approach_axis(self): return self.axis - + @property def feature_vec(self): """Returns the feature vector for the suction point. @@ -315,11 +352,11 @@ def feature_vec(self): `v = [center, axis, depth]` """ return self.center.data - + @staticmethod def from_feature_vec(v, camera_intr=None, depth=None, axis=None): """Creates a `SuctionPoint2D` instance from a feature vector and - additional parameters. + additional parameters. Parameters ---------- @@ -335,19 +372,19 @@ def from_feature_vec(v, camera_intr=None, depth=None, axis=None): # Read feature vec. center_px = v[:2] - grasp_axis = np.array([0,0,-1]) + grasp_axis = np.array([0, 0, -1]) if v.shape > 2 and axis is None: grasp_axis = v[2:5] grasp_axis = grasp_axis / np.linalg.norm(grasp_axis) elif axis is not None: grasp_axis = axis - - grasp_depth = 0.5 + + grasp_depth = 0.5 if v.shape[0] > 5 and depth is None: grasp_depth = v[5] elif depth is not None: grasp_depth = depth - + # Compute center and angle. center = Point(center_px, camera_intr.frame) return SuctionPoint2D(center, @@ -365,25 +402,29 @@ def pose(self): """ # Check intrinsics. if self.camera_intr is None: - raise ValueError("Must specify camera intrinsics to compute 3D grasp pose") + raise ValueError( + "Must specify camera intrinsics to compute 3D grasp pose") # Compute 3D grasp center in camera basis. suction_center_im = self.center.data center_px_im = Point(suction_center_im, frame=self.camera_intr.frame) - suction_center_camera = self.camera_intr.deproject_pixel(self.depth, center_px_im) + suction_center_camera = self.camera_intr.deproject_pixel( + self.depth, center_px_im) suction_center_camera = suction_center_camera.data # Compute 3D grasp axis in camera basis. suction_axis_camera = self.axis - + # Convert to 3D pose. suction_x_camera = suction_axis_camera - suction_z_camera = np.array([-suction_x_camera[1], suction_x_camera[0], 0]) + suction_z_camera = np.array( + [-suction_x_camera[1], suction_x_camera[0], 0]) if np.linalg.norm(suction_z_camera) < 1e-12: suction_z_camera = np.array([1.0, 0.0, 0.0]) suction_z_camera = suction_z_camera / np.linalg.norm(suction_z_camera) suction_y_camera = np.cross(suction_z_camera, suction_x_camera) - suction_rot_camera = np.c_[suction_x_camera, suction_y_camera, suction_z_camera] + suction_rot_camera = np.c_[suction_x_camera, suction_y_camera, + suction_z_camera] T_suction_camera = RigidTransform(rotation=suction_rot_camera, translation=suction_center_camera, @@ -408,7 +449,8 @@ def image_dist(g1, g2, alpha=1.0): Returns ------- - float: Distance between grasps. + float + Distance between grasps. """ # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) @@ -419,8 +461,9 @@ def image_dist(g1, g2, alpha=1.0): return point_dist + alpha * axis_dist + class MultiSuctionPoint2D(object): - """Multi-Cup Suction grasp in image space. + """Multi-Cup Suction grasp in image space. Equivalent to projecting a 6D pose to image space. @@ -431,22 +474,30 @@ class MultiSuctionPoint2D(object): camera_intr : :obj:`perception.CameraIntrinsics` Frame of reference for camera that the suction point corresponds to. """ + def __init__(self, pose, camera_intr=None): self._pose = pose - frame = "image" - if camera_intr is not None: - frame = camera_intr.frame + # TODO(vsatish): Confirm that this is really not needed. +# frame = "image" +# if camera_intr is not None: +# frame = camera_intr.frame - # If camera_intr is none use default primesense camera intrinsics. + # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: - self.camera_intr = CameraIntrinsics("primesense_overhead", fx=525, fy=525, cx=319.5, cy=239.5, width=640, height=480) - else: + self.camera_intr = CameraIntrinsics("primesense_overhead", + fx=525, + fy=525, + cx=319.5, + cy=239.5, + width=640, + height=480) + else: self.camera_intr = camera_intr def pose(self): return self._pose - + @property def frame(self): """The name of the frame of reference for the grasp.""" @@ -456,10 +507,11 @@ def frame(self): @property def center(self): - center_camera = Point(self._pose.translation, frame=self.camera_intr.frame) - center_px = self.camera_intr.project(center_camera) + center_camera = Point(self._pose.translation, + frame=self.camera_intr.frame) + center_px = self.camera_intr.project(center_camera) return center_px - + @property def axis(self): return self._pose.x_axis @@ -467,12 +519,12 @@ def axis(self): @property def approach_axis(self): return self.axis - + @property def approach_angle(self): """The angle between the grasp approach axis and camera optical axis. """ - dot = max(min(self.axis.dot(np.array([0,0,1])), 1.0), -1.0) + dot = max(min(self.axis.dot(np.array([0, 0, 1])), 1.0), -1.0) return np.arccos(dot) @property @@ -487,22 +539,22 @@ def angle(self): @property def depth(self): return self._pose.translation[2] - + @property def orientation(self): x_axis = self.axis y_axis = np.array([x_axis[1], -x_axis[0], 0]) if np.linalg.norm(y_axis) == 0: - y_axis = np.array([1,0,0]) + y_axis = np.array([1, 0, 0]) y_axis = y_axis / np.linalg.norm(y_axis) z_axis = np.cross(x_axis, y_axis) R = np.array([x_axis, y_axis, z_axis]).T delta_R = R.T.dot(self._pose.rotation) - orientation = np.arccos(delta_R[1,1]) - if delta_R[1,2] > 0: - orientation = 2*np.pi-orientation + orientation = np.arccos(delta_R[1, 1]) + if delta_R[1, 2] > 0: + orientation = 2 * np.pi - orientation return orientation - + @property def feature_vec(self): """Returns the feature vector for the suction point. @@ -511,12 +563,18 @@ def feature_vec(self): ---- `v = [center, axis, depth]` """ - return np.r_[self.center.data, np.cos(self.orientation), np.sin(self.orientation)] - + return np.r_[self.center.data, + np.cos(self.orientation), + np.sin(self.orientation)] + @staticmethod - def from_feature_vec(v, camera_intr=None, angle=None, depth=None, axis=None): + def from_feature_vec(v, + camera_intr=None, + angle=None, + depth=None, + axis=None): """Creates a `SuctionPoint2D` instance from a feature vector and - additional parameters. + additional parameters. Parameters ---------- @@ -541,29 +599,30 @@ def from_feature_vec(v, camera_intr=None, angle=None, depth=None, axis=None): elif angle is not None: grasp_angle = angle - grasp_axis = np.array([1,0,0]) + grasp_axis = np.array([1, 0, 0]) if axis is not None: grasp_axis = axis - - grasp_depth = 0.5 + + grasp_depth = 0.5 if depth is not None: grasp_depth = depth x_axis = grasp_axis y_axis = np.array([grasp_axis[1], -grasp_axis[0], 0]) if np.linalg.norm(y_axis) == 0: - y_axis = np.array([1,0,0]) + y_axis = np.array([1, 0, 0]) y_axis = y_axis / np.linalg.norm(y_axis) z_axis = np.cross(x_axis, y_axis) R = np.array([x_axis, y_axis, z_axis]).T R = R.dot(RigidTransform.x_axis_rotation(grasp_angle)) - t = camera_intr.deproject_pixel(grasp_depth, Point(center_px, frame=camera_intr.frame)).data + t = camera_intr.deproject_pixel( + grasp_depth, Point(center_px, frame=camera_intr.frame)).data T = RigidTransform(rotation=R, translation=t, from_frame="grasp", to_frame=camera_intr.frame) - + # Compute center and angle. return MultiSuctionPoint2D(T, camera_intr=camera_intr) @@ -584,7 +643,8 @@ def image_dist(g1, g2, alpha=1.0): Returns ------- - float: Distance between grasps. + float + Distance between grasps. """ # Point to point distances. point_dist = np.linalg.norm(g1.center.data - g2.center.data) @@ -593,4 +653,4 @@ def image_dist(g1, g2, alpha=1.0): dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0) axis_dist = np.arccos(dot) - return point_dist + alpha * axis_dist + return point_dist + alpha * axis_dist diff --git a/gqcnn/grasping/grasp_quality_function.py b/gqcnn/grasping/grasp_quality_function.py index 8beec0cd..5d5637e2 100644 --- a/gqcnn/grasping/grasp_quality_function.py +++ b/gqcnn/grasping/grasp_quality_function.py @@ -24,7 +24,10 @@ Grasp quality functions: suction quality function and parallel jaw grasping quality fuction. -Authors: Jason Liu and Jeff Mahler + +Authors +------- +Jason Liu & Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -37,17 +40,16 @@ from future.utils import with_metaclass import matplotlib.pyplot as plt import numpy as np -import scipy.ndimage.filters as snf import autolab_core.utils as utils from autolab_core import Point, PointCloud, RigidTransform, Logger -from perception import (RgbdImage, CameraIntrinsics, PointCloudImage, - ColorImage, BinaryImage, DepthImage, GrayscaleImage) +from perception import DepthImage from ..model import get_gqcnn_model, get_fc_gqcnn_model -from .grasp import Grasp2D, SuctionPoint2D +from .grasp import SuctionPoint2D from ..utils import GeneralConstants, GripperMode + class GraspQualityFunction(with_metaclass(ABCMeta, object)): """Abstract grasp quality class.""" @@ -74,13 +76,16 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Vector containing the real-valued grasp quality + :obj:`numpy.ndarray` + Vector containing the real-valued grasp quality for each candidate. """ pass + class ZeroGraspQualityFunction(object): """Null function.""" + def quality(self, state, actions, params=None): """Returns zero for all grasps. @@ -95,47 +100,58 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Vector containing the real-valued grasp quality + :obj:`numpy.ndarray` + Vector containing the real-valued grasp quality for each candidate. """ return 0.0 + class ParallelJawQualityFunction(GraspQualityFunction): """Abstract wrapper class for parallel jaw quality functions (only image - based metrics for now).""" + based metrics for now).""" + def __init__(self, config): GraspQualityFunction.__init__(self) # Read parameters. self._friction_coef = config["friction_coef"] self._max_friction_cone_angle = np.arctan(self._friction_coef) - + def friction_cone_angle(self, action): """Compute the angle between the axis and the boundaries of the - friction cone.""" + friction cone.""" if action.contact_points is None or action.contact_normals is None: - raise ValueError("Cannot compute friction cone angle without precomputed contact points and normals") - dot_prod1 = min(max(action.contact_normals[0].dot(-action.axis), -1.0), 1.0) + invalid_friction_ang_msg = ("Cannot compute friction cone angle" + " without precomputed contact points" + " and normals.") + raise ValueError(invalid_friction_ang_msg) + dot_prod1 = min(max(action.contact_normals[0].dot(-action.axis), -1.0), + 1.0) angle1 = np.arccos(dot_prod1) - dot_prod2 = min(max(action.contact_normals[1].dot(action.axis), -1.0), 1.0) + dot_prod2 = min(max(action.contact_normals[1].dot(action.axis), -1.0), + 1.0) angle2 = np.arccos(dot_prod2) return max(angle1, angle2) - + def force_closure(self, action): """Determine if the grasp is in force closure.""" - return (self.friction_cone_angle(action) < self._max_friction_cone_angle) + return (self.friction_cone_angle(action) < + self._max_friction_cone_angle) + class ComForceClosureParallelJawQualityFunction(ParallelJawQualityFunction): """Measures the distance to the estimated center of mass for antipodal - parallel-jaw grasps.""" + parallel-jaw grasps.""" + def __init__(self, config): """Create a best-fit planarity suction metric.""" self._antipodality_pctile = config["antipodality_pctile"] ParallelJawQualityFunction.__init__(self, config) - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a parallel-jaw grasp, compute the distance to the center of - mass of the grasped object. + mass of the grasped object. Parameters ---------- @@ -146,29 +162,35 @@ def quality(self, state, actions, params=None): A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. params: dict - Stores params used in computing quality. + Stores params used in computing quality. Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ # Compute antipodality. - antipodality_q = [ParallelJawQualityFunction.friction_cone_angle(self, action) for action in actions] - + antipodality_q = [ + ParallelJawQualityFunction.friction_cone_angle(self, action) + for action in actions + ] + # Compute object centroid. object_com = state.rgbd_im.center if state.segmask is not None: nonzero_px = state.segmask.nonzero_pixels() object_com = np.mean(nonzero_px, axis=0) - + # Compute negative SSE from the best fit plane for each grasp. - antipodality_thresh = abs(np.percentile(antipodality_q, 100-self._antipodality_pctile)) + antipodality_thresh = abs( + np.percentile(antipodality_q, 100 - self._antipodality_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) for i, action in enumerate(actions): q = max_q friction_cone_angle = antipodality_q[i] - force_closure = ParallelJawQualityFunction.force_closure(self, action) + force_closure = ParallelJawQualityFunction.force_closure( + self, action) if force_closure or friction_cone_angle < antipodality_thresh: grasp_center = np.array([action.center.y, action.center.x]) @@ -177,21 +199,23 @@ def quality(self, state, actions, params=None): grasp_center[1]] obj_mask = state.obj_segmask.segment_mask(grasp_obj_id) nonzero_px = obj_mask.nonzero_pixels() - object_com = np.mean(nonzero_px, axis=0) + object_com = np.mean(nonzero_px, axis=0) q = np.linalg.norm(grasp_center - object_com) if state.obj_segmask is not None and grasp_obj_id == 0: q = max_q - - q = (np.exp(-q/max_q) - np.exp(-1)) / (1 - np.exp(-1)) + + q = (np.exp(-q / max_q) - np.exp(-1)) / (1 - np.exp(-1)) qualities.append(q) return np.array(qualities) - + + class SuctionQualityFunction(GraspQualityFunction): """Abstract wrapper class for suction quality functions (only image based - metrics for now).""" + metrics for now).""" + def __init__(self, config): GraspQualityFunction.__init(self) @@ -201,18 +225,19 @@ def __init__(self, config): def _points_in_window(self, point_cloud_image, action, segmask=None): """Retrieve all points on the object in a box of size - `self._window_size`.""" + `self._window_size`.""" # Read indices. im_shape = point_cloud_image.shape - i_start = int(max(action.center.y-self._window_size//2, 0)) # TODO: Confirm div. - j_start = int(max(action.center.x-self._window_size//2, 0)) - i_end = int(min(i_start+self._window_size, im_shape[0])) - j_end = int(min(j_start+self._window_size, im_shape[1])) + i_start = int(max(action.center.y - self._window_size // 2, + 0)) # TODO: Confirm div. + j_start = int(max(action.center.x - self._window_size // 2, 0)) + i_end = int(min(i_start + self._window_size, im_shape[0])) + j_end = int(min(j_start + self._window_size, im_shape[1])) step = int(1 / self._sample_rate) # Read 3D points in the window. points = point_cloud_image[i_start:i_end:step, j_start:j_end:step] - stacked_points = points.reshape(points.shape[0]*points.shape[1], -1) + stacked_points = points.reshape(points.shape[0] * points.shape[1], -1) # Form the matrices for plane-fitting. return stacked_points @@ -228,7 +253,7 @@ def _points_to_matrices(self, points): def _best_fit_plane(self, A, b): """Find a best-fit plane of points.""" try: - w, _, _, _ = np.linalg.lstsq(A, b) + w, _, _, _ = np.linalg.lstsq(A, b) except np.linalg.LinAlgError: self._logger.warning("Could not find a best-fit plane!") raise @@ -238,15 +263,16 @@ def _sum_of_squared_residuals(self, w, A, z): """Returns the sum of squared residuals from the plane.""" return (1.0 / A.shape[0]) * np.square(np.linalg.norm(np.dot(A, w) - z)) + class BestFitPlanaritySuctionQualityFunction(SuctionQualityFunction): """A best-fit planarity suction metric.""" def __init__(self, config): SuctionQualityFunction.__init__(self, config) - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. + of the neighboring points. Parameters ---------- @@ -257,32 +283,40 @@ def quality(self, state, actions, params=None): A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. params: dict - Stores params used in computing suction quality. + Stores params used in computing suction quality. Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ qualities = [] # Deproject points. - point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) + point_cloud_image = state.camera_intr.deproject_to_image( + state.rgbd_im.depth) # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError("This function can only be used to evaluate suction quality") - - points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. + not_suction_msg = ("This function can only be used to evaluate" + " suction quality.") + raise ValueError(not_suction_msg) + + # x,y in matrix A and z is vector z. + points = self._points_in_window(point_cloud_image, + action, + segmask=state.segmask) A, b = self._points_to_matrices(points) - w = self._best_fit_plane(A, b) # vector w w/ a bias term represents a best-fit plane. + # vector w w/ a bias term represents a best-fit plane. + w = self._best_fit_plane(A, b) if params is not None and params["vis"]["plane"]: from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d mid_i = A.shape[0] // 2 pred_z = A.dot(w) - p0 = np.array([A[mid_i,0], A[mid_i,1], pred_z[mid_i]]) + p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) tx = np.array([n[1], -n[0], 0]) @@ -293,23 +327,31 @@ def quality(self, state, actions, params=None): translation=p0, from_frame="patch", to_frame="world") - + vis3d.figure() - vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0,0,1)) - vis3d.points(PointCloud(points.T), scale=0.0025, color=(1,0,0)) + vis3d.points(point_cloud_image.to_point_cloud(), + scale=0.0025, + subsample=10, + random=True, + color=(0, 0, 1)) + vis3d.points(PointCloud(points.T), + scale=0.0025, + color=(1, 0, 0)) vis3d.table(T_table_world, dim=0.01) vis3d.show() - + vis2d.figure() vis2d.imshow(state.rgbd_im.depth) vis2d.scatter(action.center.x, action.center.y, s=50, c="b") vis2d.show() - quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) # Evaluate how well best-fit plane describles all points in window. + # Evaluate how well best-fit plane describles all points in window. + quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) qualities.append(quality) return np.array(qualities) + class ApproachPlanaritySuctionQualityFunction(SuctionQualityFunction): """A approach planarity suction metric.""" @@ -323,12 +365,13 @@ def _action_to_plane(self, point_cloud_image, action): y = int(action.center.y) p_0 = point_cloud_image[y, x] n = -action.axis - w = np.array([-n[0]/n[2], -n[1]/n[2], np.dot(n,p_0)/n[2]]) # TODO: Confirm divs. + # TODO: Confirm divs. + w = np.array([-n[0] / n[2], -n[1] / n[2], np.dot(n, p_0) / n[2]]) return w - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. + of the neighboring points. Parameters ---------- @@ -343,28 +386,36 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ qualities = [] # Deproject points. - point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) + point_cloud_image = state.camera_intr.deproject_to_image( + state.rgbd_im.depth) # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError("This function can only be used to evaluate suction quality") - - points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. + not_suction_msg = ("This function can only be used to evaluate" + " suction quality.") + raise ValueError(not_suction_msg) + + # x,y in matrix A and z is vector z. + points = self._points_in_window(point_cloud_image, + action, + segmask=state.segmask) A, b = self._points_to_matrices(points) - w = self._action_to_plane(point_cloud_image, action) # vector w w/ a bias term represents a best-fit plane. + # vector w w/ a bias term represents a best-fit plane. + w = self._action_to_plane(point_cloud_image, action) if params is not None and params["vis"]["plane"]: from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d mid_i = A.shape[0] // 2 pred_z = A.dot(w) - p0 = np.array([A[mid_i,0], A[mid_i,1], pred_z[mid_i]]) + p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) tx = np.array([n[1], -n[0], 0]) @@ -372,32 +423,41 @@ def quality(self, state, actions, params=None): ty = np.cross(n, tx) R = np.array([tx, ty, n]).T - c = state.camera_intr.deproject_pixel(action.depth, action.center) - d = Point(c.data - 0.01*action.axis, frame=c.frame) + c = state.camera_intr.deproject_pixel(action.depth, + action.center) + d = Point(c.data - 0.01 * action.axis, frame=c.frame) T_table_world = RigidTransform(rotation=R, translation=p0, from_frame="patch", to_frame="world") - + vis3d.figure() - vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0,0,1)) - vis3d.points(PointCloud(points.T), scale=0.0025, color=(1,0,0)) - vis3d.points(c, scale=0.005, color=(1,1,0)) - vis3d.points(d, scale=0.005, color=(1,1,0)) + vis3d.points(point_cloud_image.to_point_cloud(), + scale=0.0025, + subsample=10, + random=True, + color=(0, 0, 1)) + vis3d.points(PointCloud(points.T), + scale=0.0025, + color=(1, 0, 0)) + vis3d.points(c, scale=0.005, color=(1, 1, 0)) + vis3d.points(d, scale=0.005, color=(1, 1, 0)) vis3d.table(T_table_world, dim=0.01) vis3d.show() - + vis2d.figure() vis2d.imshow(state.rgbd_im.depth) vis2d.scatter(action.center.x, action.center.y, s=50, c="b") vis2d.show() - quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) # Evaluate how well best-fit plane describles all points in window. + # Evaluate how well best-fit plane describles all points in window. + quality = np.exp(-self._sum_of_squared_residuals(w, A, b)) qualities.append(quality) return np.array(qualities) + class DiscApproachPlanaritySuctionQualityFunction(SuctionQualityFunction): """A approach planarity suction metric using a disc-shaped window.""" @@ -412,29 +472,30 @@ def _action_to_plane(self, point_cloud_image, action): y = int(action.center.y) p_0 = point_cloud_image[y, x] n = -action.axis - w = np.array([-n[0]/n[2], -n[1]/n[2], np.dot(n,p_0)/n[2]]) # TODO: Confirm divs. + # TODO: Confirm divs. + w = np.array([-n[0] / n[2], -n[1] / n[2], np.dot(n, p_0) / n[2]]) return w def _points_in_window(self, point_cloud_image, action, segmask=None): """Retrieve all points on the object in a disc of size - `self._window_size`.""" + `self._window_size`.""" # Compute plane. n = -action.axis - U, _, _ = np.linalg.svd(n.reshape((3,1))) - tangents = U[:,1:] + U, _, _ = np.linalg.svd(n.reshape((3, 1))) + tangents = U[:, 1:] # Read indices. im_shape = point_cloud_image.shape - i_start = int(max(action.center.y-self._window_size//2, 0)) - j_start = int(max(action.center.x-self._window_size//2, 0)) - i_end = int(min(i_start+self._window_size, im_shape[0])) - j_end = int(min(j_start+self._window_size, im_shape[1])) + i_start = int(max(action.center.y - self._window_size // 2, 0)) + j_start = int(max(action.center.x - self._window_size // 2, 0)) + i_end = int(min(i_start + self._window_size, im_shape[0])) + j_end = int(min(j_start + self._window_size, im_shape[1])) step = int(1 / self._sample_rate) # Read 3D points in the window. points = point_cloud_image[i_start:i_end:step, j_start:j_end:step] - stacked_points = points.reshape(points.shape[0]*points.shape[1], -1) - + stacked_points = points.reshape(points.shape[0] * points.shape[1], -1) + # Compute the center point. contact_point = point_cloud_image[int(action.center.y), int(action.center.x)] @@ -451,9 +512,9 @@ def _points_in_window(self, point_cloud_image, action, segmask=None): # Form the matrices for plane-fitting. return stacked_points - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. + of the neighboring points. Parameters ---------- @@ -468,21 +529,29 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ qualities = [] # Deproject points. - point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) + point_cloud_image = state.camera_intr.deproject_to_image( + state.rgbd_im.depth) # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError("This function can only be used to evaluate suction quality") - - points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. + not_suction_msg = ("This function can only be used to evaluate" + " suction quality.") + raise ValueError(not_suction_msg) + + # x,y in matrix A and z is vector z. + points = self._points_in_window(point_cloud_image, + action, + segmask=state.segmask) A, b = self._points_to_matrices(points) - w = self._action_to_plane(point_cloud_image, action) # vector w w/ a bias term represents a best-fit plane. + # vector w w/ a bias term represents a best-fit plane. + w = self._action_to_plane(point_cloud_image, action) sse = self._sum_of_squared_residuals(w, A, b) if params is not None and params["vis"]["plane"]: @@ -490,7 +559,7 @@ def quality(self, state, actions, params=None): from visualization import Visualizer3D as vis3d mid_i = A.shape[0] // 2 pred_z = A.dot(w) - p0 = np.array([A[mid_i,0], A[mid_i,1], pred_z[mid_i]]) + p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) tx = np.array([n[1], -n[0], 0]) @@ -498,35 +567,45 @@ def quality(self, state, actions, params=None): ty = np.cross(n, tx) R = np.array([tx, ty, n]).T - c = state.camera_intr.deproject_pixel(action.depth, action.center) - d = Point(c.data - 0.01*action.axis, frame=c.frame) + c = state.camera_intr.deproject_pixel(action.depth, + action.center) + d = Point(c.data - 0.01 * action.axis, frame=c.frame) T_table_world = RigidTransform(rotation=R, translation=p0, from_frame="patch", to_frame="world") - + vis3d.figure() - vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0,0,1)) - vis3d.points(PointCloud(points.T), scale=0.0025, color=(1,0,0)) - vis3d.points(c, scale=0.005, color=(1,1,0)) - vis3d.points(d, scale=0.005, color=(1,1,0)) + vis3d.points(point_cloud_image.to_point_cloud(), + scale=0.0025, + subsample=10, + random=True, + color=(0, 0, 1)) + vis3d.points(PointCloud(points.T), + scale=0.0025, + color=(1, 0, 0)) + vis3d.points(c, scale=0.005, color=(1, 1, 0)) + vis3d.points(d, scale=0.005, color=(1, 1, 0)) vis3d.table(T_table_world, dim=0.01) vis3d.show() - + vis2d.figure() vis2d.imshow(state.rgbd_im.depth) vis2d.scatter(action.center.x, action.center.y, s=50, c="b") vis2d.show() - quality = np.exp(-sse) # Evaluate how well best-fit plane describles all points in window. + # Evaluate how well best-fit plane describles all points in window. + quality = np.exp(-sse) qualities.append(quality) return np.array(qualities) -class ComApproachPlanaritySuctionQualityFunction(ApproachPlanaritySuctionQualityFunction): + +class ComApproachPlanaritySuctionQualityFunction( + ApproachPlanaritySuctionQualityFunction): """A approach planarity suction metric that ranks sufficiently planar - points by their distance to the object COM.""" + points by their distance to the object COM.""" def __init__(self, config): """Create approach planarity suction metric.""" @@ -534,9 +613,9 @@ def __init__(self, config): ApproachPlanaritySuctionQualityFunction.__init__(self, config) - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. + of the neighboring points. Parameters ---------- @@ -551,14 +630,21 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ # Compute planarity. - sse = ApproachPlanaritySuctionQualityFunction.quality(self, state, actions, params=params) + sse = ApproachPlanaritySuctionQualityFunction.quality(self, + state, + actions, + params=params) if params["vis"]["hist"]: plt.figure() - utils.histogram(sse, 100, (np.min(sse), np.max(sse)), normalized=False, plot=True) + utils.histogram(sse, + 100, (np.min(sse), np.max(sse)), + normalized=False, + plot=True) plt.show() # Compute object centroid. @@ -579,83 +665,27 @@ def quality(self, state, actions, params=None): return np.array(qualities) -class ComDiscApproachPlanaritySuctionQualityFunction(DiscApproachPlanaritySuctionQualityFunction): - """A approach planarity suction metric that ranks sufficiently planar - points by their distance to the object COM.""" - - def __init__(self, config): - """Create approach planarity suction metric.""" - self._planarity_pctile = config["planarity_pctile"] - self._planarity_abs_thresh = 0 - if "planarity_abs_thresh" in config: - self._planarity_abs_thresh = np.exp(-config["planarity_abs_thresh"]) - DiscApproachPlanaritySuctionQualityFunction.__init__(self, config) - - def quality(self, state, actions, params=None): - """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. - - Parameters - ---------- - state : :obj:`RgbdImageState` - An RgbdImageState instance that encapsulates rgbd_im, camera_intr, - segmask, full_observed. - action: :obj:`SuctionPoint2D` - A suction grasp in image space that encapsulates center, approach - direction, depth, camera_intr. - params: dict - Stores params used in computing suction quality. - - Returns - ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. - """ - # Compute planarity. - sse_q = DiscApproachPlanaritySuctionQualityFunction.quality(self, state, actions, params=params) - - if params["vis"]["hist"]: - plt.figure() - utils.histogram(sse_q, 100, (np.min(sse_q), np.max(sse_q)), normalized=False, plot=True) - plt.show() - - # Compute object centroid. - object_com = state.rgbd_im.center - if state.segmask is not None: - nonzero_px = state.segmask.nonzero_pixels() - object_com = np.mean(nonzero_px, axis=0) - - # Threshold. - planarity_thresh = abs(np.percentile(sse_q, 100-self._planarity_pctile)) - qualities = [] - max_q = max(state.rgbd_im.height, state.rgbd_im.width) - for k, action in enumerate(actions): - q = max_q - if sse_q[k] > planarity_thresh or sse_q[k] > self._planarity_abs_thresh: - grasp_center = np.array([action.center.y, action.center.x]) - q = np.linalg.norm(grasp_center - object_com) - - q = (np.exp(-q/max_q) - np.exp(-1)) / (1 - np.exp(-1)) - qualities.append(q) - - return np.array(qualities) - -class ComDiscApproachPlanaritySuctionQualityFunction(DiscApproachPlanaritySuctionQualityFunction): +class ComDiscApproachPlanaritySuctionQualityFunction( + DiscApproachPlanaritySuctionQualityFunction): """A approach planarity suction metric that ranks sufficiently planar - points by their distance to the object COM.""" + points by their distance to the object COM.""" + + # NOTE: THERE WAS A SLIGHTLY DIFFERENT DUPLICATE ABOVE. def __init__(self, config): """Create approach planarity suction metric.""" self._planarity_pctile = config["planarity_pctile"] self._planarity_abs_thresh = 0 if "planarity_abs_thresh" in config: - self._planarity_abs_thresh = np.exp(-config["planarity_abs_thresh"]) + self._planarity_abs_thresh = np.exp( + -config["planarity_abs_thresh"]) DiscApproachPlanaritySuctionQualityFunction.__init__(self, config) - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. + of the neighboring points. Parameters ---------- @@ -670,14 +700,19 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ # Compute planarity. - sse_q = DiscApproachPlanaritySuctionQualityFunction.quality(self, state, actions, params=params) + sse_q = DiscApproachPlanaritySuctionQualityFunction.quality( + self, state, actions, params=params) if params["vis"]["hist"]: plt.figure() - utils.histogram(sse_q, 100, (np.min(sse_q), np.max(sse_q)), normalized=False, plot=True) + utils.histogram(sse_q, + 100, (np.min(sse_q), np.max(sse_q)), + normalized=False, + plot=True) plt.show() # Compute object centroid. @@ -687,12 +722,14 @@ def quality(self, state, actions, params=None): object_com = np.mean(nonzero_px, axis=0) # Threshold. - planarity_thresh = abs(np.percentile(sse_q, 100-self._planarity_pctile)) + planarity_thresh = abs( + np.percentile(sse_q, 100 - self._planarity_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) for k, action in enumerate(actions): q = max_q - if sse_q[k] > planarity_thresh or sse_q[k] > self._planarity_abs_thresh: + if sse_q[k] > planarity_thresh or sse_q[ + k] > self._planarity_abs_thresh: grasp_center = np.array([action.center.y, action.center.x]) if state.obj_segmask is not None: @@ -700,15 +737,16 @@ def quality(self, state, actions, params=None): grasp_center[1]] obj_mask = state.obj_segmask.segment_mask(grasp_obj_id) nonzero_px = obj_mask.nonzero_pixels() - object_com = np.mean(nonzero_px, axis=0) + object_com = np.mean(nonzero_px, axis=0) q = np.linalg.norm(grasp_center - object_com) - q = (np.exp(-q/max_q) - np.exp(-1)) / (1 - np.exp(-1)) + q = (np.exp(-q / max_q) - np.exp(-1)) / (1 - np.exp(-1)) qualities.append(q) return np.array(qualities) - + + class GaussianCurvatureSuctionQualityFunction(SuctionQualityFunction): """A approach planarity suction metric.""" @@ -718,17 +756,17 @@ def __init__(self, config): def _points_to_matrices(self, points): """Convert a set of 3D points to an A and b matrix for regression.""" - x = points[:,0] - y = points[:,1] - A = np.c_[x, y, x*x, x*y, y*y] + x = points[:, 0] + y = points[:, 1] + A = np.c_[x, y, x * x, x * y, y * y] ones = np.ones([A.shape[0], 1]) A = np.c_[A, ones] b = points[:, 2] return A, b - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane - of the neighboring points. + of the neighboring points. Parameters ---------- @@ -743,22 +781,30 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ qualities = [] # Deproject points. - point_cloud_image = state.camera_intr.deproject_to_image(state.rgbd_im.depth) + point_cloud_image = state.camera_intr.deproject_to_image( + state.rgbd_im.depth) # Compute negative SSE from the best fit plane for each grasp. for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): - raise ValueError("This function can only be used to evaluate suction quality") - - points = self._points_in_window(point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. + not_suction_msg = ("This function can only be used to evaluate" + " suction quality.") + raise ValueError(not_suction_msg) + + # x,y in matrix A and z is vector z. + points = self._points_in_window(point_cloud_image, + action, + segmask=state.segmask) A, b = self._points_to_matrices(points) - w = self._best_fit_plane(A, b) # vector w w/ a bias term represents a best-fit plane. - + # vector w w/ a bias term represents a best-fit plane. + w = self._best_fit_plane(A, b) + # Compute curvature. fx = w[0] fy = w[1] @@ -773,7 +819,9 @@ def quality(self, state, actions, params=None): return np.array(qualities) -class DiscCurvatureSuctionQualityFunction(GaussianCurvatureSuctionQualityFunction): + +class DiscCurvatureSuctionQualityFunction( + GaussianCurvatureSuctionQualityFunction): def __init__(self, config): """Create approach planarity suction metric.""" self._radius = config["radius"] @@ -781,20 +829,19 @@ def __init__(self, config): def _points_in_window(self, point_cloud_image, action, segmask=None): """Retrieve all points on the object in a disc of size - `self._window_size`. - """ + `self._window_size`.""" # Read indices. im_shape = point_cloud_image.shape - i_start = int(max(action.center.y-self._window_size//2, 0)) - j_start = int(max(action.center.x-self._window_size//2, 0)) - i_end = int(min(i_start+self._window_size, im_shape[0])) - j_end = int(min(j_start+self._window_size, im_shape[1])) + i_start = int(max(action.center.y - self._window_size // 2, 0)) + j_start = int(max(action.center.x - self._window_size // 2, 0)) + i_end = int(min(i_start + self._window_size, im_shape[0])) + j_end = int(min(j_start + self._window_size, im_shape[1])) step = int(1 / self._sample_rate) # Read 3D points in the window. points = point_cloud_image[i_start:i_end:step, j_start:j_end:step] - stacked_points = points.reshape(points.shape[0]*points.shape[1], -1) - + stacked_points = points.reshape(points.shape[0] * points.shape[1], -1) + # Check the distance from the center point. contact_point = point_cloud_image[int(action.center.y), int(action.center.x)] @@ -804,16 +851,18 @@ def _points_in_window(self, point_cloud_image, action, segmask=None): # Form the matrices for plane-fitting. return stacked_points -class ComDiscCurvatureSuctionQualityFunction(DiscCurvatureSuctionQualityFunction): + +class ComDiscCurvatureSuctionQualityFunction( + DiscCurvatureSuctionQualityFunction): def __init__(self, config): """Create approach planarity suction metric.""" self._curvature_pctile = config["curvature_pctile"] DiscCurvatureSuctionQualityFunction.__init__(self, config) - def quality(self, state, actions, params=None): + def quality(self, state, actions, params=None): """Given a suction point, compute a score based on the Gaussian - curvature. + curvature. Parameters ---------- @@ -828,14 +877,20 @@ def quality(self, state, actions, params=None): Returns ------- - :obj:`numpy.ndarray`: Array of the quality for each grasp. + :obj:`numpy.ndarray` + Array of the quality for each grasp. """ # Compute planarity. - curvature_q = DiscCurvatureSuctionQualityFunction.quality(self, state, actions, params=params) + curvature_q = DiscCurvatureSuctionQualityFunction.quality( + self, state, actions, params=params) if params["vis"]["hist"]: plt.figure() - utils.histogram(curvature, 100, (np.min(curvature), np.max(curvature)), normalized=False, plot=True) + # NOTE: This used to be an undefined `curvature`. + utils.histogram(curvature_q, + 100, (np.min(curvature_q), np.max(curvature_q)), + normalized=False, + plot=True) plt.show() # Compute object centroid. @@ -845,7 +900,8 @@ def quality(self, state, actions, params=None): object_com = np.mean(nonzero_px, axis=0) # Threshold. - curvature_q_thresh = abs(np.percentile(curvature_q, 100-self._curvature_pctile)) + curvature_q_thresh = abs( + np.percentile(curvature_q, 100 - self._curvature_pctile)) qualities = [] max_q = max(state.rgbd_im.height, state.rgbd_im.width) for k, action in enumerate(actions): @@ -854,11 +910,12 @@ def quality(self, state, actions, params=None): grasp_center = np.array([action.center.y, action.center.x]) q = np.linalg.norm(grasp_center - object_com) - q = (np.exp(-q/max_q) - np.exp(-1)) / (1 - np.exp(-1)) + q = (np.exp(-q / max_q) - np.exp(-1)) / (1 - np.exp(-1)) qualities.append(q) return np.array(qualities) + class GQCnnQualityFunction(GraspQualityFunction): def __init__(self, config): """Create a GQCNN suction quality function.""" @@ -869,7 +926,7 @@ def __init__(self, config): self._gqcnn_model_dir = config["gqcnn_model"] self._crop_height = config["crop_height"] self._crop_width = config["crop_width"] - + # Init GQ-CNN self._gqcnn = get_gqcnn_model().load(self._gqcnn_model_dir) @@ -879,7 +936,8 @@ def __init__(self, config): def __del__(self): try: self._gqcnn.close_session() - except: + except Exception: + # TODO(vsatish): Except specific exception. pass @property @@ -933,34 +991,41 @@ def grasps_to_tensors(self, grasps, state): # Allocate tensors. tensor_start = time() - image_tensor = np.zeros([num_grasps, gqcnn_im_height, gqcnn_im_width, gqcnn_num_channels]) + image_tensor = np.zeros( + [num_grasps, gqcnn_im_height, gqcnn_im_width, gqcnn_num_channels]) pose_tensor = np.zeros([num_grasps, gqcnn_pose_dim]) scale = gqcnn_im_height / self._crop_height depth_im_scaled = depth_im.resize(scale) for i, grasp in enumerate(grasps): - translation = scale * np.array([depth_im.center[0] - grasp.center.data[1], - depth_im.center[1] - grasp.center.data[0]]) + translation = scale * np.array([ + depth_im.center[0] - grasp.center.data[1], + depth_im.center[1] - grasp.center.data[0] + ]) im_tf = depth_im_scaled im_tf = depth_im_scaled.transform(translation, grasp.angle) im_tf = im_tf.crop(gqcnn_im_height, gqcnn_im_width) - image_tensor[i,...] = im_tf.raw_data - + image_tensor[i, ...] = im_tf.raw_data + if gripper_mode == GripperMode.PARALLEL_JAW: pose_tensor[i] = grasp.depth elif gripper_mode == GripperMode.SUCTION: - pose_tensor[i,...] = np.array([grasp.depth, grasp.approach_angle]) + pose_tensor[i, ...] = np.array( + [grasp.depth, grasp.approach_angle]) elif gripper_mode == GripperMode.MULTI_SUCTION: pose_tensor[i] = grasp.depth elif gripper_mode == GripperMode.LEGACY_PARALLEL_JAW: pose_tensor[i] = grasp.depth elif gripper_mode == GripperMode.LEGACY_SUCTION: - pose_tensor[i,...] = np.array([grasp.depth, grasp.approach_angle]) + pose_tensor[i, ...] = np.array( + [grasp.depth, grasp.approach_angle]) else: - raise ValueError("Gripper mode %s not supported" %(gripper_mode)) - self._logger.debug("Tensor conversion took %.3f sec" %(time()-tensor_start)) + raise ValueError("Gripper mode %s not supported" % + (gripper_mode)) + self._logger.debug("Tensor conversion took %.3f sec" % + (time() - tensor_start)) return image_tensor, pose_tensor - def quality(self, state, actions, params): + def quality(self, state, actions, params): """Evaluate the quality of a set of actions according to a GQ-CNN. Parameters @@ -974,13 +1039,15 @@ def quality(self, state, actions, params): Returns ------- - :obj:`list` of float: Real-valued grasp quality predictions for each + :obj:`list` of float + Real-valued grasp quality predictions for each action, between 0 and 1. """ # Form tensors. tensor_start = time() image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) - self._logger.info("Image transformation took %.3f sec" %(time() - tensor_start)) + self._logger.info("Image transformation took %.3f sec" % + (time() - tensor_start)) if params is not None and params["vis"]["tf_images"]: # Read vis params. k = params["vis"]["k"] @@ -988,34 +1055,33 @@ def quality(self, state, actions, params): # Display grasp transformed images. from visualization import Visualizer2D as vis2d - vis2d.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) - for i, image_tf in enumerate(image_tensor[:k,...]): + vis2d.figure(size=(GeneralConstants.FIGSIZE, + GeneralConstants.FIGSIZE)) + for i, image_tf in enumerate(image_tensor[:k, ...]): depth = pose_tensor[i][0] - vis2d.subplot(d,d,i+1) + vis2d.subplot(d, d, i + 1) vis2d.imshow(DepthImage(image_tf)) - vis2d.title("Image %d: d=%.3f" %(i, depth)) + vis2d.title("Image %d: d=%.3f" % (i, depth)) vis2d.show() # Predict grasps. predict_start = time() output_arr = self.gqcnn.predict(image_tensor, pose_tensor) - q_values = output_arr[:,-1] - self._logger.info("Inference took %.3f sec" %(time() - predict_start)) + q_values = output_arr[:, -1] + self._logger.info("Inference took %.3f sec" % (time() - predict_start)) return q_values.tolist() + class NoMagicQualityFunction(GraspQualityFunction): def __init__(self, config): """Create a quality that uses `nomagic_net` as a quality function.""" - import json - import os - from nomagic_submission import ConvNetModel from tensorpack import SaverRestore from tensorpack.predict import OfflinePredictor from tensorpack.predict.config import PredictConfig - + GraspQualityFunction.__init(self) - + # Store parameters. self._model_path = config["gqcnn_model"] self._batch_size = config["batch_size"] @@ -1031,12 +1097,12 @@ def __init__(self, config): # Init config. model = ConvNetModel() - self._config = PredictConfig( - model=model, - session_init=SaverRestore(self._model_path), - output_names=["prob"]) + self._config = PredictConfig(model=model, + session_init=SaverRestore( + self._model_path), + output_names=["prob"]) self._predictor = OfflinePredictor(self._config) - + @property def gqcnn(self): """Returns the GQ-CNN.""" @@ -1060,8 +1126,10 @@ def grasps_to_tensors(self, grasps, state): Returns ------- - :obj:`numpy.ndarray`: 4D numpy tensor of image to be predicted. - :obj:`numpy.ndarray`: 2D numpy tensor of depth values. + :obj:`numpy.ndarray` + 4D numpy tensor of image to be predicted. + :obj:`numpy.ndarray` + 2D numpy tensor of depth values. """ # Parse params. gqcnn_im_height = self._im_height @@ -1074,36 +1142,45 @@ def grasps_to_tensors(self, grasps, state): # Allocate tensors. tensor_start = time() - image_tensor = np.zeros([num_grasps, gqcnn_im_height, - gqcnn_im_width, gqcnn_num_channels]) + image_tensor = np.zeros( + [num_grasps, gqcnn_im_height, gqcnn_im_width, gqcnn_num_channels]) pose_tensor = np.zeros([num_grasps, gqcnn_pose_dim]) scale = gqcnn_im_height / self._crop_height depth_im_scaled = depth_im.resize(scale) for i, grasp in enumerate(grasps): - translation = scale * np.array([depth_im.center[0] - grasp.center.data[1], - depth_im.center[1] - grasp.center.data[0]]) + translation = scale * np.array([ + depth_im.center[0] - grasp.center.data[1], + depth_im.center[1] - grasp.center.data[0] + ]) im_tf = depth_im_scaled im_tf = depth_im_scaled.transform(translation, grasp.angle) im_tf = im_tf.crop(gqcnn_im_height, gqcnn_im_width) - im_encoded = cv2.imencode(".png", np.uint8(im_tf.raw_data*255))[1].tostring() - im_decoded = cv2.imdecode(np.frombuffer(im_encoded, np.uint8), 0) / 255.0 - image_tensor[i,:,:,0] = ((im_decoded - self._data_mean) / self._data_std) - + im_encoded = cv2.imencode(".png", np.uint8(im_tf.raw_data * + 255))[1].tostring() + im_decoded = cv2.imdecode(np.frombuffer(im_encoded, np.uint8), + 0) / 255.0 + image_tensor[i, :, :, 0] = ((im_decoded - self._data_mean) / + self._data_std) + if gripper_mode == GripperMode.PARALLEL_JAW: pose_tensor[i] = grasp.depth elif gripper_mode == GripperMode.SUCTION: - pose_tensor[i,...] = np.array([grasp.depth, grasp.approach_angle]) + pose_tensor[i, ...] = np.array( + [grasp.depth, grasp.approach_angle]) elif gripper_mode == GripperMode.LEGACY_PARALLEL_JAW: pose_tensor[i] = grasp.depth elif gripper_mode == GripperMode.LEGACY_SUCTION: - pose_tensor[i,...] = np.array([grasp.depth, grasp.approach_angle]) + pose_tensor[i, ...] = np.array( + [grasp.depth, grasp.approach_angle]) else: - raise ValueError("Gripper mode %s not supported" %(gripper_mode)) - self._logger.debug("Tensor conversion took %.3f sec" %(time()-tensor_start)) + raise ValueError("Gripper mode %s not supported" % + (gripper_mode)) + self._logger.debug("Tensor conversion took %.3f sec" % + (time() - tensor_start)) return image_tensor, pose_tensor - def quality(self, state, actions, params): + def quality(self, state, actions, params): """Evaluate the quality of a set of actions according to a GQ-CNN. Parameters @@ -1117,7 +1194,9 @@ def quality(self, state, actions, params): Returns ------- - :obj:`list` of float: Real-valued grasp quality predictions for each action, between 0 and 1. + :obj:`list` of float + Real-valued grasp quality predictions for each action, between 0 + and 1. """ # Form tensors. image_tensor, pose_tensor = self.grasps_to_tensors(actions, state) @@ -1128,12 +1207,13 @@ def quality(self, state, actions, params): # Display grasp transformed images. from visualization import Visualizer2D as vis2d - vis2d.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) - for i, image_tf in enumerate(image_tensor[:k,...]): + vis2d.figure(size=(GeneralConstants.FIGSIZE, + GeneralConstants.FIGSIZE)) + for i, image_tf in enumerate(image_tensor[:k, ...]): depth = pose_tensor[i][0] - vis2d.subplot(d,d,i+1) + vis2d.subplot(d, d, i + 1) vis2d.imshow(DepthImage(image_tf)) - vis2d.title("Image %d: d=%.3f" %(i, depth)) + vis2d.title("Image %d: d=%.3f" % (i, depth)) vis2d.show() # Predict grasps. @@ -1144,13 +1224,17 @@ def quality(self, state, actions, params): cur_i = 0 end_i = cur_i + min(self._batch_size, num_actions - cur_i) while cur_i < num_actions: - output_arr[cur_i:end_i,:] = self.gqcnn(image_tensor[cur_i:end_i,:,:,0], pose_tensor[cur_i:end_i,0], null_arr)[0] + output_arr[cur_i:end_i, :] = self.gqcnn( + image_tensor[cur_i:end_i, :, :, 0], + pose_tensor[cur_i:end_i, 0], null_arr)[0] cur_i = end_i end_i = cur_i + min(self._batch_size, num_actions - cur_i) - q_values = output_arr[:,-1] - self._logger.debug("Prediction took %.3f sec" %(time()-predict_start)) + q_values = output_arr[:, -1] + self._logger.debug("Prediction took %.3f sec" % + (time() - predict_start)) return q_values.tolist() + class FCGQCnnQualityFunction(GraspQualityFunction): def __init__(self, config): """Grasp quality function using the fully-convolutional gqcnn.""" @@ -1163,7 +1247,8 @@ def __init__(self, config): self._fully_conv_config = config["fully_conv_gqcnn_config"] # Init fcgqcnn. - self._fcgqcnn = get_fc_gqcnn_model(backend=self._backend).load(self._model_dir, self._fully_conv_config) + self._fcgqcnn = get_fc_gqcnn_model(backend=self._backend).load( + self._model_dir, self._fully_conv_config) # Open Tensorflow session for fcgqcnn. self._fcgqcnn.open_session() @@ -1171,7 +1256,8 @@ def __init__(self, config): def __del__(self): try: self._fcgqcnn.close_session() - except: + except Exception: + # TODO(vsatish): Except specific exception. pass @property @@ -1184,11 +1270,13 @@ def config(self): """Returns the FC-GQCNN quality function parameters.""" return self._config - def quality(self, images, depths, params=None): + def quality(self, images, depths, params=None): return self._fcgqcnn.predict(images, depths) + class GraspQualityFunctionFactory(object): """Factory for grasp quality functions.""" + @staticmethod def quality_function(metric_type, config): if metric_type == "zero": @@ -1218,4 +1306,5 @@ def quality_function(metric_type, config): elif metric_type == "fcgqcnn": return FCGQCnnQualityFunction(config) else: - raise ValueError("Grasp function type %s not supported!" %(metric_type)) + raise ValueError("Grasp function type %s not supported!" % + (metric_type)) diff --git a/gqcnn/grasping/image_grasp_sampler.py b/gqcnn/grasping/image_grasp_sampler.py index 99162beb..2a0e188b 100644 --- a/gqcnn/grasping/image_grasp_sampler.py +++ b/gqcnn/grasping/image_grasp_sampler.py @@ -24,17 +24,18 @@ Classes for sampling a set of grasps directly from images to generate data for a neural network. -Author: Jeff Mahler, Sherdil Niyaz + +Author +------ +Jeff Mahler & Sherdil Niyaz """ from __future__ import absolute_import from __future__ import division from __future__ import print_function from abc import ABCMeta, abstractmethod -import copy -import os import random -from time import sleep, time +from time import time from future.utils import with_metaclass import matplotlib.pyplot as plt @@ -42,23 +43,21 @@ import scipy.ndimage.filters as snf import scipy.spatial.distance as ssd import scipy.stats as ss -import sklearn.mixture from autolab_core import Point, RigidTransform, Logger -from perception import (BinaryImage, ColorImage, DepthImage, - RgbdImage, GdImage) +from perception import (DepthImage, RgbdImage, GdImage) from visualization import Visualizer2D as vis from .grasp import Grasp2D, SuctionPoint2D, MultiSuctionPoint2D -from ..utils import NoAntipodalPairsFoundException + def force_closure(p1, p2, n1, n2, mu): """Computes whether or not the point and normal pairs are in force - closure.""" - # Line between the contacts. + closure.""" + # Line between the contacts. v = p2 - p1 v = v / np.linalg.norm(v) - + # Compute cone membership. alpha = np.arctan(mu) dot_1 = max(min(n1.dot(-v), 1.0), -1.0) @@ -67,21 +66,24 @@ def force_closure(p1, p2, n1, n2, mu): in_cone_2 = (np.arccos(dot_2) < alpha) return (in_cone_1 and in_cone_2) + class DepthSamplingMode(object): """Modes for sampling grasp depth.""" UNIFORM = "uniform" MIN = "min" MAX = "max" + class ImageGraspSampler(with_metaclass(ABCMeta, object)): """Wraps image to crane grasp candidate generation for easy deployment of - GQ-CNN. + GQ-CNN. Attributes ---------- config : :obj:`autolab_core.YamlConfig` A dictionary-like object containing the parameters of the sampler. """ + def __init__(self, config): # Set params. self._config = config @@ -89,11 +91,16 @@ def __init__(self, config): # Setup logger. self._logger = Logger.get_logger(self.__class__.__name__) - def sample(self, rgbd_im, camera_intr, num_samples, - segmask=None, seed=None, visualize=False, + def sample(self, + rgbd_im, + camera_intr, + num_samples, + segmask=None, + seed=None, + visualize=False, constraint_fn=None): """Samples a set of 2D grasps from a given RGB-D image. - + Parameters ---------- rgbd_im : :obj:`perception.RgbdImage` @@ -113,7 +120,8 @@ def sample(self, rgbd_im, camera_intr, num_samples, Returns ------- - :obj:`list` of :obj:`Grasp2D`: The list of grasps in image space. + :obj:`list` of :obj:`Grasp2D` + The list of grasps in image space. """ # Set random seed for determinism. if seed is not None: @@ -123,17 +131,26 @@ def sample(self, rgbd_im, camera_intr, num_samples, # Sample an initial set of grasps (without depth). self._logger.debug("Sampling 2d candidates") sampling_start = time() - grasps = self._sample(rgbd_im, camera_intr, num_samples, - segmask=segmask, visualize=visualize, + grasps = self._sample(rgbd_im, + camera_intr, + num_samples, + segmask=segmask, + visualize=visualize, constraint_fn=constraint_fn) sampling_stop = time() - self._logger.debug("Sampled %d grasps from image" %(len(grasps))) - self._logger.debug("Sampling grasps took %.3f sec" %(sampling_stop - sampling_start)) + self._logger.debug("Sampled %d grasps from image" % (len(grasps))) + self._logger.debug("Sampling grasps took %.3f sec" % + (sampling_stop - sampling_start)) return grasps @abstractmethod - def _sample(self, rgbd_im, camera_intr, num_samples, segmask=None, - visualize=False, constraint_fn=None): + def _sample(self, + rgbd_im, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image. Subclasses must override. @@ -152,13 +169,15 @@ def _sample(self, rgbd_im, camera_intr, num_samples, segmask=None, Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. - + Returns ------- - :obj:`list` of :obj:`Grasp2D`: List of 2D grasp candidates. + :obj:`list` of :obj:`Grasp2D` + List of 2D grasp candidates. """ pass - + + class AntipodalDepthImageGraspSampler(ImageGraspSampler): """Grasp sampler for antipodal point pairs from depth image gradients. @@ -204,6 +223,7 @@ class AntipodalDepthImageGraspSampler(ImageGraspSampler): depth_sampling_mode : str Name of depth sampling mode (uniform, min, max). """ + def __init__(self, config, gripper_width=np.inf): # Init superclass. ImageGraspSampler.__init__(self, config) @@ -212,7 +232,8 @@ def __init__(self, config, gripper_width=np.inf): self._gripper_width = self._config["gripper_width"] self._friction_coef = self._config["friction_coef"] self._depth_grad_thresh = self._config["depth_grad_thresh"] - self._depth_grad_gaussian_sigma = self._config["depth_grad_gaussian_sigma"] + self._depth_grad_gaussian_sigma = self._config[ + "depth_grad_gaussian_sigma"] self._downsample_rate = self._config["downsample_rate"] self._rescale_factor = 1.0 / self._downsample_rate self._max_rejection_samples = self._config["max_rejection_samples"] @@ -220,7 +241,7 @@ def __init__(self, config, gripper_width=np.inf): self._min_num_edge_pixels = 0 if "min_num_edge_pixels" in self._config: self._min_num_edge_pixels = self._config["min_num_edge_pixels"] - + # Distance thresholds for rejection sampling. self._max_dist_from_center = self._config["max_dist_from_center"] self._min_dist_from_boundary = self._config["min_dist_from_boundary"] @@ -228,7 +249,8 @@ def __init__(self, config, gripper_width=np.inf): self._angle_dist_weight = self._config["angle_dist_weight"] # Depth sampling params. - self._depth_samples_per_grasp = max(self._config["depth_samples_per_grasp"], 1) + self._depth_samples_per_grasp = max( + self._config["depth_samples_per_grasp"], 1) self._min_depth_offset = self._config["min_depth_offset"] self._max_depth_offset = self._config["max_depth_offset"] self._h = self._config["depth_sample_win_height"] @@ -238,11 +260,12 @@ def __init__(self, config, gripper_width=np.inf): # Perturbation. self._grasp_center_sigma = 0.0 if "grasp_center_sigma" in self._config: - self._grasp_center_sigma = self._config["grasp_center_sigma"] + self._grasp_center_sigma = self._config["grasp_center_sigma"] self._grasp_angle_sigma = 0.0 if "grasp_angle_sigma" in self._config: - self._grasp_angle_sigma = np.deg2rad(self._config["grasp_angle_sigma"]) - + self._grasp_angle_sigma = np.deg2rad( + self._config["grasp_angle_sigma"]) + def _surface_normals(self, depth_im, edge_pixels): """Return an array of the surface normals at the edge pixels.""" # Compute the gradients. @@ -255,9 +278,9 @@ def _surface_normals(self, depth_im, edge_pixels): dy = grad[0][pix[0], pix[1]] normal_vec = np.array([dy, dx]) if np.linalg.norm(normal_vec) == 0: - normal_vec = np.array([1,0]) + normal_vec = np.array([1, 0]) normal_vec = normal_vec / np.linalg.norm(normal_vec) - normals[i,:] = normal_vec + normals[i, :] = normal_vec return normals @@ -265,21 +288,27 @@ def _sample_depth(self, min_depth, max_depth): """Samples a depth value between the min and max.""" depth_sample = max_depth if self._depth_sampling_mode == DepthSamplingMode.UNIFORM: - depth_sample = min_depth + (max_depth - min_depth) * np.random.rand() + depth_sample = min_depth + (max_depth - + min_depth) * np.random.rand() elif self._depth_sampling_mode == DepthSamplingMode.MIN: depth_sample = min_depth return depth_sample - def _sample(self, image, camera_intr, num_samples, segmask=None, - visualize=False, constraint_fn=None): + def _sample(self, + image, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image. Parameters ---------- - image : :obj:`perception.RgbdImage` or "perception.DepthImage" or "perception.GdImage" - RGB-D or D image to sample from + image : :obj:`perception.RgbdImage` or :obj:`perception.DepthImage` or :obj:`perception.GdImage` # noqa: E501 + RGB-D or Depth image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` - Intrinsics of the camera that captured the images + Intrinsics of the camera that captured the images. num_samples : int Number of grasps to sample segmask : :obj:`perception.BinaryImage` @@ -288,7 +317,7 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. - + Returns ------- :obj:`list` of :obj:`Grasp2D` @@ -299,16 +328,25 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, elif isinstance(image, DepthImage): depth_im = image else: - raise ValueError("image type must be one of [RgbdImage, DepthImage, GdImage]") + raise ValueError( + "image type must be one of [RgbdImage, DepthImage, GdImage]") # Sample antipodal pairs in image space. - grasps = self._sample_antipodal_grasps(depth_im, camera_intr, num_samples, - segmask=segmask, visualize=visualize, + grasps = self._sample_antipodal_grasps(depth_im, + camera_intr, + num_samples, + segmask=segmask, + visualize=visualize, constraint_fn=constraint_fn) return grasps - def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, - segmask=None, visualize=False, constraint_fn=None): + def _sample_antipodal_grasps(self, + depth_im, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image by finding depth edges, then uniformly sampling point pairs and keeping only antipodal grasps with width less than the maximum allowable. @@ -327,7 +365,7 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. - + Returns ------- :obj:`list` of :obj:`Grasp2D` @@ -339,68 +377,83 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, sigma=self._depth_grad_gaussian_sigma) scale_factor = self._rescale_factor depth_im_downsampled = depth_im.resize(scale_factor) - depth_im_threshed = depth_im_downsampled.threshold_gradients(self._depth_grad_thresh) + depth_im_threshed = depth_im_downsampled.threshold_gradients( + self._depth_grad_thresh) edge_pixels = (1.0 / scale_factor) * depth_im_threshed.zero_pixels() edge_pixels = edge_pixels.astype(np.int16) depth_im_mask = depth_im.copy() if segmask is not None: - edge_pixels = np.array([p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)]) + edge_pixels = np.array( + [p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)]) depth_im_mask = depth_im.mask_binary(segmask) # Re-threshold edges if there are too few. if edge_pixels.shape[0] < self._min_num_edge_pixels: self._logger.info("Too few edge pixels!") - depth_im_threshed = depth_im.threshold_gradients(self._depth_grad_thresh) - edge_pixels = depth_im_threshed.zero_pixels() + depth_im_threshed = depth_im.threshold_gradients( + self._depth_grad_thresh) + edge_pixels = depth_im_threshed.zero_pixels() edge_pixels = edge_pixels.astype(np.int16) depth_im_mask = depth_im.copy() if segmask is not None: - edge_pixels = np.array([p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0)]) + edge_pixels = np.array([ + p for p in edge_pixels if np.any(segmask[p[0], p[1]] > 0) + ]) depth_im_mask = depth_im.mask_binary(segmask) num_pixels = edge_pixels.shape[0] - self._logger.debug("Depth edge detection took %.3f sec" %(time() - edge_start)) - self._logger.debug("Found %d edge pixels" %(num_pixels)) + self._logger.debug("Depth edge detection took %.3f sec" % + (time() - edge_start)) + self._logger.debug("Found %d edge pixels" % (num_pixels)) # Compute point cloud. point_cloud_im = camera_intr.deproject_to_image(depth_im_mask) - + # Compute_max_depth. depth_data = depth_im_mask.data[depth_im_mask.data > 0] if depth_data.shape[0] == 0: return [] - + min_depth = np.min(depth_data) + self._min_depth_offset max_depth = np.max(depth_data) + self._max_depth_offset # Compute surface normals. normal_start = time() edge_normals = self._surface_normals(depth_im, edge_pixels) - self._logger.debug("Normal computation took %.3f sec" %(time() - normal_start)) + self._logger.debug("Normal computation took %.3f sec" % + (time() - normal_start)) if visualize: - edge_pixels = edge_pixels[::2,:] - edge_normals = edge_normals[::2,:] + edge_pixels = edge_pixels[::2, :] + edge_normals = edge_normals[::2, :] vis.figure() - vis.subplot(1,3,1) + vis.subplot(1, 3, 1) vis.imshow(depth_im) if num_pixels > 0: - vis.scatter(edge_pixels[:,1], edge_pixels[:,0], s=2, c="b") + vis.scatter(edge_pixels[:, 1], edge_pixels[:, 0], s=2, c="b") X = [pix[1] for pix in edge_pixels] Y = [pix[0] for pix in edge_pixels] - U = [3*pix[1] for pix in edge_normals] - V = [-3*pix[0] for pix in edge_normals] - plt.quiver(X, Y, U, V, units="x", scale=0.25, width=0.5, zorder=2, color="r") + U = [3 * pix[1] for pix in edge_normals] + V = [-3 * pix[0] for pix in edge_normals] + plt.quiver(X, + Y, + U, + V, + units="x", + scale=0.25, + width=0.5, + zorder=2, + color="r") vis.title("Edge pixels and normals") - vis.subplot(1,3,2) + vis.subplot(1, 3, 2) vis.imshow(depth_im_threshed) vis.title("Edge map") - vis.subplot(1,3,3) + vis.subplot(1, 3, 3) vis.imshow(segmask) vis.title("Segmask") vis.show() @@ -408,17 +461,22 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, # Exit if no edge pixels. if num_pixels == 0: return [] - + # Form set of valid candidate point pairs. pruning_start = time() - max_grasp_width_px = Grasp2D(Point(np.zeros(2)), 0.0, min_depth, - width = self._gripper_width, + max_grasp_width_px = Grasp2D(Point(np.zeros(2)), + 0.0, + min_depth, + width=self._gripper_width, camera_intr=camera_intr).width_px normal_ip = edge_normals.dot(edge_normals.T) dists = ssd.squareform(ssd.pdist(edge_pixels)) - valid_indices = np.where((normal_ip < -np.cos(np.arctan(self._friction_coef))) & (dists < max_grasp_width_px) & (dists > 0.0)) + valid_indices = np.where( + (normal_ip < -np.cos(np.arctan(self._friction_coef))) + & (dists < max_grasp_width_px) & (dists > 0.0)) valid_indices = np.c_[valid_indices[0], valid_indices[1]] - self._logger.debug("Normal pruning %.3f sec" %(time() - pruning_start)) + self._logger.debug("Normal pruning %.3f sec" % + (time() - pruning_start)) # Raise exception if no antipodal pairs. num_pairs = valid_indices.shape[0] @@ -426,13 +484,13 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, return [] # Prune out grasps. - contact_points1 = edge_pixels[valid_indices[:,0],:] - contact_points2 = edge_pixels[valid_indices[:,1],:] - contact_normals1 = edge_normals[valid_indices[:,0],:] - contact_normals2 = edge_normals[valid_indices[:,1],:] + contact_points1 = edge_pixels[valid_indices[:, 0], :] + contact_points2 = edge_pixels[valid_indices[:, 1], :] + contact_normals1 = edge_normals[valid_indices[:, 0], :] + contact_normals2 = edge_normals[valid_indices[:, 1], :] v = contact_points1 - contact_points2 v_norm = np.linalg.norm(v, axis=1) - v = v / np.tile(v_norm[:,np.newaxis], [1,2]) + v = v / np.tile(v_norm[:, np.newaxis], [1, 2]) ip1 = np.sum(contact_normals1 * v, axis=1) ip2 = np.sum(contact_normals2 * (-v), axis=1) ip1[ip1 > 1.0] = 1.0 @@ -452,7 +510,8 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, grasp_indices = np.random.choice(antipodal_indices, size=sample_size, replace=False) - self._logger.debug("Grasp comp took %.3f sec" %(time() - pruning_start)) + self._logger.debug("Grasp comp took %.3f sec" % + (time() - pruning_start)) # Compute grasps. sample_start = time() @@ -460,11 +519,11 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, grasps = [] while k < sample_size and len(grasps) < num_samples: grasp_ind = grasp_indices[k] - p1 = contact_points1[grasp_ind,:] - p2 = contact_points2[grasp_ind,:] - n1 = contact_normals1[grasp_ind,:] - n2 = contact_normals2[grasp_ind,:] - width = np.linalg.norm(p1 - p2) + p1 = contact_points1[grasp_ind, :] + p2 = contact_points2[grasp_ind, :] + n1 = contact_normals1[grasp_ind, :] + n2 = contact_normals2[grasp_ind, :] + # width = np.linalg.norm(p1 - p2) k += 1 # Compute center and axis. @@ -474,39 +533,49 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, grasp_theta = np.pi / 2 if grasp_axis[1] != 0: grasp_theta = np.arctan2(grasp_axis[0], grasp_axis[1]) - grasp_center_pt = Point(np.array([grasp_center[1], grasp_center[0]]), frame=camera_intr.frame) + grasp_center_pt = Point(np.array( + [grasp_center[1], grasp_center[0]]), + frame=camera_intr.frame) # Compute grasp points in 3D. x1 = point_cloud_im[p1[0], p1[1]] x2 = point_cloud_im[p2[0], p2[1]] - if np.linalg.norm(x2-x1) > self._gripper_width: + if np.linalg.norm(x2 - x1) > self._gripper_width: continue - + # Perturb. if self._grasp_center_sigma > 0.0: - grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs(cov=self._grasp_center_sigma*np.diag(np.ones(2))) + grasp_center_pt = grasp_center_pt + ss.multivariate_normal.rvs( + cov=self._grasp_center_sigma * np.diag(np.ones(2))) if self._grasp_angle_sigma > 0.0: - grasp_theta = grasp_theta + ss.norm.rvs(scale=self._grasp_angle_sigma) + grasp_theta = grasp_theta + ss.norm.rvs( + scale=self._grasp_angle_sigma) # Check center px dist from boundary. - if grasp_center[0] < self._min_dist_from_boundary or \ - grasp_center[1] < self._min_dist_from_boundary or \ - grasp_center[0] > depth_im.height - self._min_dist_from_boundary or \ - grasp_center[1] > depth_im.width - self._min_dist_from_boundary: + if (grasp_center[0] < self._min_dist_from_boundary + or grasp_center[1] < self._min_dist_from_boundary + or grasp_center[0] > + depth_im.height - self._min_dist_from_boundary + or grasp_center[1] > + depth_im.width - self._min_dist_from_boundary): continue - + # Sample depths. for i in range(self._depth_samples_per_grasp): # Get depth in the neighborhood of the center pixel. - depth_win = depth_im.data[grasp_center[0]-self._h:grasp_center[0]+self._h, grasp_center[1]-self._w:grasp_center[1]+self._w] + depth_win = depth_im.data[grasp_center[0] - + self._h:grasp_center[0] + + self._h, grasp_center[1] - + self._w:grasp_center[1] + self._w] center_depth = np.min(depth_win) if center_depth == 0 or np.isnan(center_depth): continue - + # Sample depth between the min and max. min_depth = center_depth + self._min_depth_offset max_depth = center_depth + self._max_depth_offset - sample_depth = min_depth + (max_depth - min_depth) * np.random.rand() + sample_depth = min_depth + (max_depth - + min_depth) * np.random.rand() candidate_grasp = Grasp2D(grasp_center_pt, grasp_theta, sample_depth, @@ -522,12 +591,14 @@ def _sample_antipodal_grasps(self, depth_im, camera_intr, num_samples, vis.scatter(p1[1], p1[0], c="b", s=25) vis.scatter(p2[1], p2[0], c="b", s=25) vis.show() - + grasps.append(candidate_grasp) + # Return sampled grasps. - self._logger.debug("Loop took %.3f sec" %(time() - sample_start)) + self._logger.debug("Loop took %.3f sec" % (time() - sample_start)) return grasps + class DepthImageSuctionPointSampler(ImageGraspSampler): """Grasp sampler for suction points from depth images. @@ -558,12 +629,14 @@ class DepthImageSuctionPointSampler(ImageGraspSampler): depth_gaussian_sigma : float Sigma used for pre-smoothing the depth image for better gradients. """ + def __init__(self, config): # Init superclass. ImageGraspSampler.__init__(self, config) # Read params. - self._max_suction_dir_optical_axis_angle = np.deg2rad(self._config["max_suction_dir_optical_axis_angle"]) + self._max_suction_dir_optical_axis_angle = np.deg2rad( + self._config["max_suction_dir_optical_axis_angle"]) self._max_dist_from_center = self._config["max_dist_from_center"] self._min_dist_from_boundary = self._config["min_dist_from_boundary"] self._max_num_samples = self._config["max_num_samples"] @@ -571,12 +644,12 @@ def __init__(self, config): self._min_theta = -np.deg2rad(self._config["delta_theta"]) self._max_theta = np.deg2rad(self._config["delta_theta"]) self._theta_rv = ss.uniform(loc=self._min_theta, - scale=self._max_theta-self._min_theta) + scale=self._max_theta - self._min_theta) self._min_phi = -np.deg2rad(self._config["delta_phi"]) self._max_phi = np.deg2rad(self._config["delta_phi"]) self._phi_rv = ss.uniform(loc=self._min_phi, - scale=self._max_phi-self._min_phi) + scale=self._max_phi - self._min_phi) self._mean_depth = 0.0 if "mean_depth" in self._config: @@ -587,9 +660,14 @@ def __init__(self, config): self._min_suction_dist = self._config["min_suction_dist"] self._angle_dist_weight = self._config["angle_dist_weight"] self._depth_gaussian_sigma = self._config["depth_gaussian_sigma"] - - def _sample(self, image, camera_intr, num_samples, segmask=None, - visualize=False, constraint_fn=None): + + def _sample(self, + image, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image. Parameters @@ -604,7 +682,7 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, Binary image segmenting out the object of interest. visualize : bool Whether or not to show intermediate samples (for debugging). - + Returns ------- :obj:`list` of :obj:`Grasp2D` @@ -615,15 +693,25 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, elif isinstance(image, DepthImage): depth_im = image else: - raise ValueError("image type must be one of [RgbdImage, DepthImage, GdImage]") + raise ValueError( + "image type must be one of [RgbdImage, DepthImage, GdImage]") # Sample antipodal pairs in image space. - grasps = self._sample_suction_points(depth_im, camera_intr, num_samples, - segmask=segmask, visualize=visualize, constraint_fn=constraint_fn) + grasps = self._sample_suction_points(depth_im, + camera_intr, + num_samples, + segmask=segmask, + visualize=visualize, + constraint_fn=constraint_fn) return grasps - def _sample_suction_points(self, depth_im, camera_intr, num_samples, - segmask=None, visualize=False, constraint_fn=None): + def _sample_suction_points(self, + depth_im, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D suction point candidates from a depth image by choosing points on an object surface uniformly at random and then sampling around the surface normal. @@ -640,7 +728,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, Binary image segmenting out the object of interest. visualize : bool Whether or not to show intermediate samples (for debugging). - + Returns ------- :obj:`list` of :obj:`SuctionPoint2D` @@ -655,13 +743,13 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, depth_im_mask = depth_im.copy() if segmask is not None: depth_im_mask = depth_im.mask_binary(segmask) - self._logger.debug("Filtering took %.3f sec" %(time() - filter_start)) - + self._logger.debug("Filtering took %.3f sec" % (time() - filter_start)) + if visualize: vis.figure() - vis.subplot(1,2,1) + vis.subplot(1, 2, 1) vis.imshow(depth_im) - vis.subplot(1,2,2) + vis.subplot(1, 2, 2) vis.imshow(depth_im_mask) vis.show() @@ -673,7 +761,8 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, num_nonzero_px = nonzero_px.shape[0] if num_nonzero_px == 0: return [] - self._logger.debug("Normal cloud took %.3f sec" %(time() - cloud_start)) + self._logger.debug("Normal cloud took %.3f sec" % + (time() - cloud_start)) # Randomly sample points and add to image. sample_start = time() @@ -684,34 +773,40 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, size=sample_size, replace=False) while k < sample_size and len(suction_points) < num_samples: - # Sample a point uniformly at random. + # Sample a point uniformly at random. ind = indices[k] - center_px = np.array([nonzero_px[ind,1], nonzero_px[ind,0]]) + center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]]) center = Point(center_px, frame=camera_intr.frame) axis = -normal_cloud_im[center.y, center.x] depth = point_cloud_im[center.y, center.x][2] - + # Update number of tries. k += 1 # Check center px dist from boundary. - if center_px[0] < self._min_dist_from_boundary or \ - center_px[1] < self._min_dist_from_boundary or \ - center_px[1] > depth_im.height - self._min_dist_from_boundary or \ - center_px[0] > depth_im.width - self._min_dist_from_boundary: - continue - + if (center_px[0] < self._min_dist_from_boundary + or center_px[1] < self._min_dist_from_boundary + or center_px[1] > + depth_im.height - self._min_dist_from_boundary + or center_px[0] > + depth_im.width - self._min_dist_from_boundary): + continue + # Perturb depth. delta_depth = self._depth_rv.rvs(size=1)[0] depth = depth + delta_depth - # Keep if the angle between the camera optical axis and the suction direction is less than a threshold. - dot = max(min(axis.dot(np.array([0,0,1])), 1.0), -1.0) + # Keep if the angle between the camera optical axis and the suction + # direction is less than a threshold. + dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0) psi = np.arccos(dot) if psi < self._max_suction_dir_optical_axis_angle: # Create candidate grasp. - candidate = SuctionPoint2D(center, axis, depth, camera_intr=camera_intr) + candidate = SuctionPoint2D(center, + axis, + depth, + camera_intr=camera_intr) # Check constraint satisfaction. if constraint_fn is None or constraint_fn(candidate): @@ -722,9 +817,10 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, vis.show() suction_points.append(candidate) - self._logger.debug("Loop took %.3f sec" %(time() - sample_start)) + self._logger.debug("Loop took %.3f sec" % (time() - sample_start)) return suction_points + class DepthImageMultiSuctionPointSampler(ImageGraspSampler): """Grasp sampler for suction points from depth images. @@ -735,26 +831,34 @@ class DepthImageMultiSuctionPointSampler(ImageGraspSampler): Other Parameters ---------------- max_suction_dir_optical_axis_angle : float - Maximum angle, in degrees, between the suction approach axis and the camera optical axis. + Maximum angle, in degrees, between the suction approach axis and the + camera optical axis. delta_theta : float - Maximum deviation from zero for the aziumth angle of a rotational perturbation to the surface normal (for sample diversity). + Maximum deviation from zero for the aziumth angle of a rotational + perturbation to the surface normal (for sample diversity). delta_phi : float - Maximum deviation from zero for the elevation angle of a rotational perturbation to the surface normal (for sample diversity). + Maximum deviation from zero for the elevation angle of a rotational + perturbation to the surface normal (for sample diversity). sigma_depth : float - Standard deviation for a normal distribution over depth values (for sample diversity). + Standard deviation for a normal distribution over depth values (for + sample diversity). min_suction_dist : float - Minimum admissible distance between suction points (for sample diversity). + Minimum admissible distance between suction points (for sample + diversity). angle_dist_weight : float - Amount to weight the angle difference in suction point distance computation. + Amount to weight the angle difference in suction point distance + computation. depth_gaussian_sigma : float Sigma used for pre-smoothing the depth image for better gradients. """ + def __init__(self, config): # Init superclass. ImageGraspSampler.__init__(self, config) # Read params. - self._max_suction_dir_optical_axis_angle = np.deg2rad(self._config["max_suction_dir_optical_axis_angle"]) + self._max_suction_dir_optical_axis_angle = np.deg2rad( + self._config["max_suction_dir_optical_axis_angle"]) self._max_dist_from_center = self._config["max_dist_from_center"] self._min_dist_from_boundary = self._config["min_dist_from_boundary"] self._max_num_samples = self._config["max_num_samples"] @@ -762,12 +866,12 @@ def __init__(self, config): self._min_theta = -np.deg2rad(self._config["delta_theta"]) self._max_theta = np.deg2rad(self._config["delta_theta"]) self._theta_rv = ss.uniform(loc=self._min_theta, - scale=self._max_theta-self._min_theta) + scale=self._max_theta - self._min_theta) self._min_phi = -np.deg2rad(self._config["delta_phi"]) self._max_phi = np.deg2rad(self._config["delta_phi"]) self._phi_rv = ss.uniform(loc=self._min_phi, - scale=self._max_phi-self._min_phi) + scale=self._max_phi - self._min_phi) self._mean_depth = 0.0 if "mean_depth" in self._config: @@ -778,14 +882,19 @@ def __init__(self, config): self._min_suction_dist = self._config["min_suction_dist"] self._angle_dist_weight = self._config["angle_dist_weight"] self._depth_gaussian_sigma = self._config["depth_gaussian_sigma"] - - def _sample(self, image, camera_intr, num_samples, segmask=None, - visualize=False, constraint_fn=None): + + def _sample(self, + image, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D grasp candidates from a depth image. Parameters ---------- - image : :obj:`perception.RgbdImage` or "perception.DepthImage" + image : :obj:`perception.RgbdImage` or `perception.DepthImage` RGB-D or D image to sample from. camera_intr : :obj:`perception.CameraIntrinsics` Intrinsics of the camera that captured the images. @@ -797,7 +906,7 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. - + Returns ------- :obj:`list` of :obj:`Grasp2D` @@ -808,16 +917,25 @@ def _sample(self, image, camera_intr, num_samples, segmask=None, elif isinstance(image, DepthImage): depth_im = image else: - raise ValueError("image type must be one of [RgbdImage, DepthImage, GdImage]") + raise ValueError( + "image type must be one of [RgbdImage, DepthImage, GdImage]") # Sample antipodal pairs in image space. - grasps = self._sample_suction_points(depth_im, camera_intr, num_samples, - segmask=segmask, visualize=visualize, + grasps = self._sample_suction_points(depth_im, + camera_intr, + num_samples, + segmask=segmask, + visualize=visualize, constraint_fn=constraint_fn) return grasps - def _sample_suction_points(self, depth_im, camera_intr, num_samples, - segmask=None, visualize=False, constraint_fn=None): + def _sample_suction_points(self, + depth_im, + camera_intr, + num_samples, + segmask=None, + visualize=False, + constraint_fn=None): """Sample a set of 2D suction point candidates from a depth image by choosing points on an object surface uniformly at random and then sampling around the surface normal. @@ -836,7 +954,7 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, Whether or not to show intermediate samples (for debugging). constraint_fn : :obj:`GraspConstraintFn` Constraint function to apply to grasps. - + Returns ------- :obj:`list` of :obj:`SuctionPoint2D` @@ -851,13 +969,13 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, depth_im_mask = depth_im.copy() if segmask is not None: depth_im_mask = depth_im.mask_binary(segmask) - self._logger.debug("Filtering took %.3f sec" %(time() - filter_start)) - + self._logger.debug("Filtering took %.3f sec" % (time() - filter_start)) + if visualize: vis.figure() - vis.subplot(1,2,1) + vis.subplot(1, 2, 1) vis.imshow(depth_im) - vis.subplot(1,2,2) + vis.subplot(1, 2, 2) vis.imshow(depth_im_mask) vis.show() @@ -869,7 +987,8 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, num_nonzero_px = nonzero_px.shape[0] if num_nonzero_px == 0: return [] - self._logger.debug("Normal cloud took %.3f sec" %(time() - cloud_start)) + self._logger.debug("Normal cloud took %.3f sec" % + (time() - cloud_start)) # Randomly sample points and add to image. sample_start = time() @@ -880,12 +999,12 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, size=sample_size, replace=False) while k < sample_size and len(suction_points) < num_samples: - # Sample a point uniformly at random. + # Sample a point uniformly at random. ind = indices[k] - center_px = np.array([nonzero_px[ind,1], nonzero_px[ind,0]]) + center_px = np.array([nonzero_px[ind, 1], nonzero_px[ind, 0]]) center = Point(center_px, frame=camera_intr.frame) axis = -normal_cloud_im[center.y, center.x] - depth = point_cloud_im[center.y, center.x][2] + # depth = point_cloud_im[center.y, center.x][2] orientation = 2 * np.pi * np.random.rand() # Update number of tries. @@ -899,28 +1018,30 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, x_axis = axis y_axis = np.array([axis[1], -axis[0], 0]) if np.linalg.norm(y_axis) == 0: - y_axis = np.array([1,0,0]) + y_axis = np.array([1, 0, 0]) y_axis = y_axis / np.linalg.norm(y_axis) z_axis = np.cross(x_axis, y_axis) R = np.array([x_axis, y_axis, z_axis]).T - R_orig = np.copy(R) + # R_orig = np.copy(R) R = R.dot(RigidTransform.x_axis_rotation(orientation)) t = point_cloud_im[center.y, center.x] pose = RigidTransform(rotation=R, translation=t, from_frame="grasp", to_frame=camera_intr.frame) - - + # Check center px dist from boundary. - if center_px[0] < self._min_dist_from_boundary or \ - center_px[1] < self._min_dist_from_boundary or \ - center_px[1] > depth_im.height - self._min_dist_from_boundary or \ - center_px[0] > depth_im.width - self._min_dist_from_boundary: - continue - - # Keep if the angle between the camera optical axis and the suction direction is less than a threshold. - dot = max(min(axis.dot(np.array([0,0,1])), 1.0), -1.0) + if (center_px[0] < self._min_dist_from_boundary + or center_px[1] < self._min_dist_from_boundary + or center_px[1] > + depth_im.height - self._min_dist_from_boundary + or center_px[0] > + depth_im.width - self._min_dist_from_boundary): + continue + + # Keep if the angle between the camera optical axis and the suction + # direction is less than a threshold. + dot = max(min(axis.dot(np.array([0, 0, 1])), 1.0), -1.0) psi = np.arccos(dot) if psi < self._max_suction_dir_optical_axis_angle: @@ -936,11 +1057,13 @@ def _sample_suction_points(self, depth_im, camera_intr, num_samples, vis.show() suction_points.append(candidate) - self._logger.debug("Loop took %.3f sec" %(time() - sample_start)) + self._logger.debug("Loop took %.3f sec" % (time() - sample_start)) return suction_points - + + class ImageGraspSamplerFactory(object): """Factory for image grasp samplers.""" + @staticmethod def sampler(sampler_type, config): if sampler_type == "antipodal_depth": @@ -950,4 +1073,5 @@ def sampler(sampler_type, config): elif sampler_type == "multi_suction": return DepthImageMultiSuctionPointSampler(config) else: - raise ValueError("Image grasp sampler type %s not supported!" %(sampler_type)) + raise ValueError("Image grasp sampler type %s not supported!" % + (sampler_type)) diff --git a/gqcnn/grasping/policy/__init__.py b/gqcnn/grasping/policy/__init__.py index 17971154..68debf8a 100644 --- a/gqcnn/grasping/policy/__init__.py +++ b/gqcnn/grasping/policy/__init__.py @@ -29,7 +29,11 @@ from .fc_policy import (FullyConvolutionalGraspingPolicyParallelJaw, FullyConvolutionalGraspingPolicySuction) from .policy import (RobustGraspingPolicy, CrossEntropyRobustGraspingPolicy, - PriorityCompositeGraspingPolicy, RgbdImageState, - GraspAction, UniformRandomGraspingPolicy) + RgbdImageState, GraspAction, UniformRandomGraspingPolicy) -__all__ = ["FullyConvolutionalGraspingPolicyParallelJaw", "FullyConvolutionalGraspingPolicySuction", "RobustGraspingPolicy", "CrossEntropyRobustGraspingPolicy", "UniformRandomGraspingPolicy", "RgbdImageState", "GraspAction"] +__all__ = [ + "FullyConvolutionalGraspingPolicyParallelJaw", + "FullyConvolutionalGraspingPolicySuction", "RobustGraspingPolicy", + "CrossEntropyRobustGraspingPolicy", "UniformRandomGraspingPolicy", + "RgbdImageState", "GraspAction" +] diff --git a/gqcnn/grasping/policy/enums.py b/gqcnn/grasping/policy/enums.py index d9e87ed8..ecd283c8 100644 --- a/gqcnn/grasping/policy/enums.py +++ b/gqcnn/grasping/policy/enums.py @@ -23,12 +23,16 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Enums for GQ-CNN policies. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division from __future__ import print_function + class SamplingMethod(object): TOP_K = "top_k" UNIFORM = "uniform" diff --git a/gqcnn/grasping/policy/fc_policy.py b/gqcnn/grasping/policy/fc_policy.py index d6f58b8b..4dc848a5 100644 --- a/gqcnn/grasping/policy/fc_policy.py +++ b/gqcnn/grasping/policy/fc_policy.py @@ -23,14 +23,16 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Fully-Convolutional GQ-CNN grasping policies. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division from __future__ import print_function from abc import abstractmethod, ABCMeta -import math import os from future.utils import with_metaclass @@ -42,15 +44,16 @@ from visualization import Visualizer2D as vis from ...grasping import Grasp2D, SuctionPoint2D -from ...utils import NoValidGraspsException +from ...utils import GeneralConstants, NoValidGraspsException from .enums import SamplingMethod from .policy import GraspingPolicy, GraspAction -class FullyConvolutionalGraspingPolicy(with_metaclass(ABCMeta, GraspingPolicy)): + +class FullyConvolutionalGraspingPolicy(with_metaclass(ABCMeta, + GraspingPolicy)): """Abstract grasp sampling policy class using Fully-Convolutional GQ-CNN - network. - """ + network.""" def __init__(self, cfg, filters=None): """ @@ -83,7 +86,7 @@ def __init__(self, cfg, filters=None): self._vis_config = self._cfg["policy_vis"] self._vis_scale = self._vis_config["scale"] self._vis_show_axis = self._vis_config["show_axis"] - + self._num_vis_samples = self._vis_config["num_samples"] self._vis_actions_2d = self._vis_config["actions_2d"] self._vis_actions_3d = self._vis_config["actions_3d"] @@ -91,27 +94,40 @@ def __init__(self, cfg, filters=None): self._vis_affordance_map = self._vis_config["affordance_map"] self._vis_output_dir = None - if "output_dir" in self._vis_config: # If this exists in the config then all visualizations will be logged here instead of displayed. + # If this exists in the config then all visualizations will be logged + # here instead of displayed. + if "output_dir" in self._vis_config: self._vis_output_dir = self._vis_config["output_dir"] self._state_counter = 0 def _unpack_state(self, state): """Unpack information from the provided `RgbdImageState`.""" - return state.rgbd_im.depth, state.rgbd_im.depth._data, state.segmask.raw_data, state.camera_intr # TODO(vsatish): Don"t access raw depth data like this. - + # TODO(vsatish): Don't access raw depth data like this. + return (state.rgbd_im.depth, state.rgbd_im.depth._data, + state.segmask.raw_data, state.camera_intr) + def _mask_predictions(self, preds, raw_segmask): """Mask the given predictions with the given segmask, setting the rest - to 0.0. - """ + to 0.0.""" preds_masked = np.zeros_like(preds) - raw_segmask_cropped = raw_segmask[self._gqcnn_recep_h // 2:raw_segmask.shape[0] - self._gqcnn_recep_h // 2, self._gqcnn_recep_w // 2:raw_segmask.shape[1] - self._gqcnn_recep_w // 2, 0] - raw_segmask_downsampled = raw_segmask_cropped[::self._gqcnn_stride, ::self._gqcnn_stride] + raw_segmask_cropped = raw_segmask[self._gqcnn_recep_h // + 2:raw_segmask.shape[0] - + self._gqcnn_recep_h // + 2, self._gqcnn_recep_w // + 2:raw_segmask.shape[1] - + self._gqcnn_recep_w // 2, 0] + raw_segmask_downsampled = raw_segmask_cropped[::self._gqcnn_stride, :: + self._gqcnn_stride] if raw_segmask_downsampled.shape[0] != preds.shape[1]: raw_segmask_downsampled_new = np.zeros(preds.shape[1:3]) - raw_segmask_downsampled_new[:raw_segmask_downsampled.shape[0], :raw_segmask_downsampled.shape[1]] = raw_segmask_downsampled + raw_segmask_downsampled_new[:raw_segmask_downsampled. + shape[0], :raw_segmask_downsampled. + shape[1]] = raw_segmask_downsampled raw_segmask_downsampled = raw_segmask_downsampled_new nonzero_mask_ind = np.where(raw_segmask_downsampled > 0) - preds_masked[:, nonzero_mask_ind[0], nonzero_mask_ind[1]] = preds[:, nonzero_mask_ind[0], nonzero_mask_ind[1]] + preds_masked[:, nonzero_mask_ind[0], + nonzero_mask_ind[1]] = preds[:, nonzero_mask_ind[0], + nonzero_mask_ind[1]] return preds_masked def _sample_predictions(self, preds, num_actions): @@ -123,51 +139,76 @@ def _sample_predictions(self, preds, num_actions): pred_ind_flat = self._sample_predictions_flat(preds_flat, num_actions) pred_ind = np.zeros((num_actions, len(preds.shape)), dtype=np.int32) for idx in range(num_actions): - pred_ind[idx, 0] = pred_ind_flat[idx] // (dim2 * dim1 * dim3) - pred_ind[idx, 1] = (pred_ind_flat[idx] - (pred_ind[idx, 0] * (dim2 * dim1 * dim3))) // (dim2 * dim3) - pred_ind[idx, 2] = (pred_ind_flat[idx] - (pred_ind[idx, 0] * (dim2 * dim1 * dim3)) - (pred_ind[idx, 1] * (dim2 * dim3))) // dim3 - pred_ind[idx, 3] = (pred_ind_flat[idx] - (pred_ind[idx, 0] * (dim2 * dim1 * dim3)) - (pred_ind[idx, 1] * (dim2 * dim3))) % dim3 + pred_ind[idx, 0] = pred_ind_flat[idx] // (dim2 * dim1 * dim3) + pred_ind[idx, 1] = (pred_ind_flat[idx] - + (pred_ind[idx, 0] * + (dim2 * dim1 * dim3))) // (dim2 * dim3) + pred_ind[idx, 2] = (pred_ind_flat[idx] - (pred_ind[idx, 0] * + (dim2 * dim1 * dim3)) - + (pred_ind[idx, 1] * (dim2 * dim3))) // dim3 + pred_ind[idx, 3] = (pred_ind_flat[idx] - (pred_ind[idx, 0] * + (dim2 * dim1 * dim3)) - + (pred_ind[idx, 1] * (dim2 * dim3))) % dim3 return pred_ind def _sample_predictions_flat(self, preds_flat, num_samples): """Helper function to do the actual sampling.""" - if num_samples == 1: # `argmax` is faster than `argpartition` for special case of single sample. + if num_samples == 1: + # `argmax` is faster than `argpartition` for special case of single + # sample. if self._sampling_method == SamplingMethod.TOP_K: return [np.argmax(preds_flat)] elif self._sampling_method == SamplingMethod.UNIFORM: - nonzero_ind = np.where(preds_flat > 0)[0] + nonzero_ind = np.where(preds_flat > 0)[0] return np.random.choice(nonzero_ind) else: - raise ValueError("Invalid sampling method: {}".format(self._sampling_method)) + raise ValueError("Invalid sampling method: {}".format( + self._sampling_method)) else: if self._sampling_method == "top_k": - return np.argpartition(preds_flat, -1 * num_samples)[-1 * num_samples:] + return np.argpartition(preds_flat, + -1 * num_samples)[-1 * num_samples:] elif self._sampling_method == "uniform": nonzero_ind = np.where(preds_flat > 0)[0] if nonzero_ind.shape[0] == 0: - raise NoValidGraspsException("No grasps with nonzero quality") + raise NoValidGraspsException( + "No grasps with nonzero quality") return np.random.choice(nonzero_ind, size=num_samples) else: - raise ValueError("Invalid sampling method: {}".format(self._sampling_method)) + raise ValueError("Invalid sampling method: {}".format( + self._sampling_method)) @abstractmethod - def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): + def _get_actions(self, preds, ind, images, depths, camera_intr, + num_actions): """Generate the actions to be returned.""" pass @abstractmethod - def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, num_actions): + def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, + num_actions): """Visualize the actions in 3D.""" pass @abstractmethod - def _visualize_affordance_map(self, preds, depth_im, scale, plot_max=True, output_dir=None): + def _visualize_affordance_map(self, + preds, + depth_im, + scale, + plot_max=True, + output_dir=None): """Visualize an affordance map of the network predictions overlayed on - the depth image. - """ + the depth image.""" pass - def _visualize_2d(self, actions, preds, wrapped_depth_im, num_actions, scale, show_axis, output_dir=None): + def _visualize_2d(self, + actions, + preds, + wrapped_depth_im, + num_actions, + scale, + show_axis, + output_dir=None): """Visualize the actions in 2D.""" self._logger.info("Visualizing actions in 2d...") @@ -175,7 +216,10 @@ def _visualize_2d(self, actions, preds, wrapped_depth_im, num_actions, scale, sh vis.figure() vis.imshow(wrapped_depth_im) for i in range(num_actions): - vis.grasp(actions[i].grasp, scale=scale, show_axis=show_axis, color=plt.cm.RdYlGn(actions[i].q_value)) + vis.grasp(actions[i].grasp, + scale=scale, + show_axis=show_axis, + color=plt.cm.RdYlGn(actions[i].q_value)) vis.title("Top {} Grasps".format(num_actions)) if output_dir is not None: vis.savefig(os.path.join(output_dir, "top_grasps.png")) @@ -188,7 +232,9 @@ def _filter(self, actions): valid = True for filter_name, is_valid in self._filters.items(): if not is_valid(action.grasp): - self._logger.info("Grasp {} is not valid with filter {}".format(action.grasp, filter_name)) + self._logger.info( + "Grasp {} is not valid with filter {}".format( + action.grasp, filter_name)) valid = False break if valid: @@ -198,47 +244,62 @@ def _filter(self, actions): @abstractmethod def _gen_images_and_depths(self, depth, segmask): """Generate inputs for the grasp quality function.""" - pass + pass def _action(self, state, num_actions=1): """Plan action(s).""" if self._filter_grasps: - assert self._filters is not None, "Trying to filter grasps but no filters were provided!" - assert num_actions == 1, "Filtering support is only implemented for single actions!" + assert self._filters is not None, ("Trying to filter grasps but no" + " filters were provided!") + assert num_actions == 1, ("Filtering support is only implemented" + " for single actions!") num_actions = self._max_grasps_to_filter # Set up log dir for state visualizations. state_output_dir = None if self._vis_output_dir is not None: - state_output_dir = os.path.join(self._vis_output_dir, "state_{}".format(str(self._state_counter).zfill(5))) + state_output_dir = os.path.join( + self._vis_output_dir, + "state_{}".format(str(self._state_counter).zfill(5))) if not os.path.exists(state_output_dir): os.makedirs(state_output_dir) self._state_counter += 1 # Unpack the `RgbdImageState`. - wrapped_depth, raw_depth, raw_seg, camera_intr = self._unpack_state(state) + wrapped_depth, raw_depth, raw_seg, camera_intr = self._unpack_state( + state) # Predict. images, depths = self._gen_images_and_depths(raw_depth, raw_seg) preds = self._grasp_quality_fn.quality(images, depths) - # Get success probablility predictions only (this is needed because the output of the network is pairs of (p_failure, p_success)). + # Get success probablility predictions only (this is needed because the + # output of the network is pairs of (p_failure, p_success)). preds_success_only = preds[:, :, :, 1::2] - - # Mask predicted success probabilities with the cropped and downsampled object segmask so we only sample grasps on the objects. - preds_success_only = self._mask_predictions(preds_success_only, raw_seg) - # If we want to visualize more than one action, we have to sample more. - num_actions_to_sample = self._num_vis_samples if (self._vis_actions_2d or self._vis_actions_3d) else num_actions # TODO(vsatish): If this is used with the "top_k" sampling method, the final returned action is not the best because the argpartition does not sort the partitioned indices. Fix this. + # Mask predicted success probabilities with the cropped and downsampled + # object segmask so we only sample grasps on the objects. + preds_success_only = self._mask_predictions(preds_success_only, + raw_seg) - if self._sampling_method == SamplingMethod.TOP_K and self._num_vis_samples: + # If we want to visualize more than one action, we have to sample more. + # TODO(vsatish): If this is used with the "top_k" sampling method, the + # final returned action is not the best because the argpartition does + # not sort the partitioned indices. Fix this. + num_actions_to_sample = self._num_vis_samples if ( + self._vis_actions_2d or self._vis_actions_3d) else num_actions + + if (self._sampling_method == SamplingMethod.TOP_K + and self._num_vis_samples): self._logger.warning("FINAL GRASP RETURNED IS NOT THE BEST!") # Sample num_actions_to_sample indices from the success predictions. - sampled_ind = self._sample_predictions(preds_success_only, num_actions_to_sample) + sampled_ind = self._sample_predictions(preds_success_only, + num_actions_to_sample) # Wrap actions to be returned. - actions = self._get_actions(preds_success_only, sampled_ind, images, depths, camera_intr, num_actions_to_sample) + actions = self._get_actions(preds_success_only, sampled_ind, images, + depths, camera_intr, num_actions_to_sample) # Filter grasps. if self._filter_grasps: @@ -248,14 +309,25 @@ def _action(self, state, num_actions=1): # Visualize. if self._vis_actions_3d: self._logger.info("Generating 3D Visualization...") - self._visualize_3d(actions, wrapped_depth, camera_intr, num_actions_to_sample) + self._visualize_3d(actions, wrapped_depth, camera_intr, + num_actions_to_sample) if self._vis_actions_2d: self._logger.info("Generating 2D visualization...") - self._visualize_2d(actions, preds_success_only, wrapped_depth, num_actions_to_sample, self._vis_scale, self._vis_show_axis, output_dir=state_output_dir) + self._visualize_2d(actions, + preds_success_only, + wrapped_depth, + num_actions_to_sample, + self._vis_scale, + self._vis_show_axis, + output_dir=state_output_dir) if self._vis_affordance_map: - self._visualize_affordance_map(preds_success_only, wrapped_depth, self._vis_scale, output_dir=state_output_dir) + self._visualize_affordance_map(preds_success_only, + wrapped_depth, + self._vis_scale, + output_dir=state_output_dir) - return actions[-1] if (self._filter_grasps or num_actions == 1) else actions[-(num_actions+1):] + return actions[-1] if (self._filter_grasps or num_actions == 1 + ) else actions[-(num_actions + 1):] def action_set(self, state, num_actions): """Plan a set of actions. @@ -272,10 +344,16 @@ def action_set(self, state, num_actions): list of :obj:`gqcnn.GraspAction` The planned grasps. """ - return [action.grasp for action in self._action(state, num_actions=num_actions)] + return [ + action.grasp + for action in self._action(state, num_actions=num_actions) + ] + -class FullyConvolutionalGraspingPolicyParallelJaw(FullyConvolutionalGraspingPolicy): +class FullyConvolutionalGraspingPolicyParallelJaw( + FullyConvolutionalGraspingPolicy): """Parallel jaw grasp sampling policy using the FC-GQ-CNN.""" + def __init__(self, cfg, filters=None): """ Parameters @@ -283,7 +361,7 @@ def __init__(self, cfg, filters=None): cfg : dict Python dictionary of policy configuration parameters. filters : dict - Python dictionary of functions to apply to filter invalid grasps. + Python dictionary of functions to apply to filter invalid grasps. """ FullyConvolutionalGraspingPolicy.__init__(self, cfg, filters=filters) @@ -291,7 +369,7 @@ def __init__(self, cfg, filters=None): # Depth sampling parameters. self._num_depth_bins = self._cfg["num_depth_bins"] - self._depth_offset = 0.0 # Hard offset from the workspace. + self._depth_offset = 0.0 # Hard offset from the workspace. if "depth_offset" in self._cfg: self._depth_offset = self._cfg["depth_offset"] @@ -299,31 +377,48 @@ def _sample_depths(self, raw_depth_im, raw_seg): """Sample depths from the raw depth image.""" max_depth = np.max(raw_depth_im) + self._depth_offset - # For sampling the min depth, we only sample from the portion of the depth image in the object segmask because sometimes the rim of the bin is not properly subtracted out of the depth image. + # For sampling the min depth, we only sample from the portion of the + # depth image in the object segmask because sometimes the rim of the + # bin is not properly subtracted out of the depth image. raw_depth_im_segmented = np.ones_like(raw_depth_im) - raw_depth_im_segmented[np.where(raw_seg > 0)] = raw_depth_im[np.where(raw_seg > 0)] + raw_depth_im_segmented[np.where(raw_seg > 0)] = raw_depth_im[np.where( + raw_seg > 0)] min_depth = np.min(raw_depth_im_segmented) + self._depth_offset depth_bin_width = (max_depth - min_depth) / self._num_depth_bins - depths = np.zeros((self._num_depth_bins, 1)) + depths = np.zeros((self._num_depth_bins, 1)) for i in range(self._num_depth_bins): - depths[i][0] = min_depth + (i * depth_bin_width + depth_bin_width / 2) + depths[i][0] = min_depth + (i * depth_bin_width + + depth_bin_width / 2) return depths - def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): + def _get_actions(self, preds, ind, images, depths, camera_intr, + num_actions): """Generate the actions to be returned.""" actions = [] - ang_bin_width = GeneralConstants.PI / preds.shape[-1] # TODO(vsatish): These should use the max angle instead. + # TODO(vsatish): These should use the max angle instead. + ang_bin_width = GeneralConstants.PI / preds.shape[-1] for i in range(num_actions): im_idx = ind[i, 0] h_idx = ind[i, 1] w_idx = ind[i, 2] ang_idx = ind[i, 3] - center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2])) - ang = GeneralConstants.PI / 2 - (ang_idx * ang_bin_width + ang_bin_width / 2) + center = Point( + np.asarray([ + w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, + h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2 + ])) + ang = GeneralConstants.PI / 2 - (ang_idx * ang_bin_width + + ang_bin_width / 2) depth = depths[im_idx, 0] - grasp = Grasp2D(center, ang, depth, width=self._gripper_width, camera_intr=camera_intr) - grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, ang_idx], DepthImage(images[im_idx])) + grasp = Grasp2D(center, + ang, + depth, + width=self._gripper_width, + camera_intr=camera_intr) + grasp_action = GraspAction(grasp, + preds[im_idx, h_idx, w_idx, ang_idx], + DepthImage(images[im_idx])) actions.append(grasp_action) return actions @@ -333,19 +428,23 @@ def _gen_images_and_depths(self, depth, segmask): images = np.tile(np.asarray([depth]), (self._num_depth_bins, 1, 1, 1)) return images, depths - def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, num_actions): + def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, + num_actions): """Visualize the actions in 3D.""" raise NotImplementedError def _visualize_affordance_map(self, preds, depth_im): """Visualize an affordance map of the network predictions overlayed on - the depth image. - """ + the depth image.""" raise NotImplementedError -class FullyConvolutionalGraspingPolicySuction(FullyConvolutionalGraspingPolicy): + +class FullyConvolutionalGraspingPolicySuction(FullyConvolutionalGraspingPolicy + ): """Suction grasp sampling policy using the FC-GQ-CNN.""" - def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): + + def _get_actions(self, preds, ind, images, depths, camera_intr, + num_actions): """Generate the actions to be returned.""" depth_im = DepthImage(images[0], frame=camera_intr.frame) point_cloud_im = camera_intr.deproject_to_image(depth_im) @@ -356,44 +455,69 @@ def _get_actions(self, preds, ind, images, depths, camera_intr, num_actions): im_idx = ind[i, 0] h_idx = ind[i, 1] w_idx = ind[i, 2] - center = Point(np.asarray([w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2])) + center = Point( + np.asarray([ + w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2, + h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2 + ])) axis = -normal_cloud_im[center.y, center.x] if np.linalg.norm(axis) == 0: continue depth = depth_im[center.y, center.x, 0] if depth == 0.0: continue - grasp = SuctionPoint2D(center, axis=axis, depth=depth, camera_intr=camera_intr) - grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, 0], DepthImage(images[im_idx])) + grasp = SuctionPoint2D(center, + axis=axis, + depth=depth, + camera_intr=camera_intr) + grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, 0], + DepthImage(images[im_idx])) actions.append(grasp_action) return actions - def _visualize_affordance_map(self, preds, depth_im, scale, plot_max=True, output_dir=None): + def _visualize_affordance_map(self, + preds, + depth_im, + scale, + plot_max=True, + output_dir=None): """Visualize an affordance map of the network predictions overlayed on - the depth image. - """ + the depth image.""" self._logger.info("Visualizing affordance map...") affordance_map = preds[0, ..., 0] - tf_depth_im = depth_im.crop(depth_im.shape[0] - self._gqcnn_recep_h, depth_im.shape[1] - self._gqcnn_recep_w).resize(1.0 / self._gqcnn_stride) + tf_depth_im = depth_im.crop( + depth_im.shape[0] - self._gqcnn_recep_h, depth_im.shape[1] - + self._gqcnn_recep_w).resize(1.0 / self._gqcnn_stride) # Plot. vis.figure() vis.imshow(tf_depth_im) - plt.imshow(affordance_map, cmap=plt.cm.RdYlGn, alpha=0.3, vmin=0.0, vmax=1.0) + plt.imshow(affordance_map, + cmap=plt.cm.RdYlGn, + alpha=0.3, + vmin=0.0, + vmax=1.0) if plot_max: - affordance_argmax = np.unravel_index(np.argmax(affordance_map), affordance_map.shape) - plt.scatter(affordance_argmax[1], affordance_argmax[0], c="black", marker=".", s=scale*25) + affordance_argmax = np.unravel_index(np.argmax(affordance_map), + affordance_map.shape) + plt.scatter(affordance_argmax[1], + affordance_argmax[0], + c="black", + marker=".", + s=scale * 25) vis.title("Grasp Affordance Map") if output_dir is not None: vis.savefig(os.path.join(output_dir, "grasp_affordance_map.png")) else: vis.show() - + def _gen_images_and_depths(self, depth, segmask): """Extend the image to a 4D tensor.""" - return np.expand_dims(depth, 0), np.array([-1]) # TODO(vsatish): Depth should be made an optional input to the network. - - def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, num_actions): + # TODO(vsatish): Depth should be made an optional input to the network. + return np.expand_dims(depth, 0), np.array([-1]) + + def _visualize_3d(self, actions, wrapped_depth_im, camera_intr, + num_actions): """Visualize the actions in 3D.""" raise NotImplementedError diff --git a/gqcnn/grasping/policy/policy.py b/gqcnn/grasping/policy/policy.py index 05167190..ec5855e8 100644 --- a/gqcnn/grasping/policy/policy.py +++ b/gqcnn/grasping/policy/policy.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Grasping policies. -Author: Jeff Mahler + +Author +------ +Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -40,12 +43,13 @@ import matplotlib.pyplot as plt import numpy as np import scipy.ndimage.filters as snf +import scipy.stats as ss from sklearn.mixture import GaussianMixture import autolab_core.utils as utils from autolab_core import Point, Logger -from perception import (BinaryImage, ColorImage, DepthImage, - RgbdImage, SegmentationImage, CameraIntrinsics) +from perception import (BinaryImage, ColorImage, DepthImage, RgbdImage, + SegmentationImage, CameraIntrinsics) from visualization import Visualizer2D as vis from ..constraint_fn import GraspConstraintFnFactory @@ -53,12 +57,15 @@ from ..grasp_quality_function import (GraspQualityFunctionFactory, GQCnnQualityFunction) from ..image_grasp_sampler import ImageGraspSamplerFactory -from ...utils import GeneralConstants, GripperMode, NoValidGraspsException +from ...utils import GeneralConstants, NoValidGraspsException + class RgbdImageState(object): """State to encapsulate RGB-D images.""" - def __init__(self, rgbd_im, camera_intr, + def __init__(self, + rgbd_im, + camera_intr, segmask=None, obj_segmask=None, fully_observed=None): @@ -118,7 +125,7 @@ def load(save_dir): The directory to load from. """ if not os.path.exists(save_dir): - raise ValueError("Directory %s does not exist!" %(save_dir)) + raise ValueError("Directory %s does not exist!" % (save_dir)) color_image_filename = os.path.join(save_dir, "color.png") depth_image_filename = os.path.join(save_dir, "depth.npy") camera_intr_filename = os.path.join(save_dir, "camera.intr") @@ -130,11 +137,13 @@ def load(save_dir): depth = DepthImage.open(depth_image_filename, frame=camera_intr.frame) segmask = None if os.path.exists(segmask_filename): - segmask = BinaryImage.open(segmask_filename, frame=camera_intr.frame) + segmask = BinaryImage.open(segmask_filename, + frame=camera_intr.frame) obj_segmask = None if os.path.exists(obj_segmask_filename): - obj_segmask = SegmentationImage.open(obj_segmask_filename, frame=camera_intr.frame) - fully_observed = None + obj_segmask = SegmentationImage.open(obj_segmask_filename, + frame=camera_intr.frame) + fully_observed = None if os.path.exists(state_filename): fully_observed = pkl.load(open(state_filename, "rb")) return RgbdImageState(RgbdImage.from_color_and_depth(color, depth), @@ -142,10 +151,12 @@ def load(save_dir): segmask=segmask, obj_segmask=obj_segmask, fully_observed=fully_observed) - + + class GraspAction(object): """Action to encapsulate grasps. """ + def __init__(self, grasp, q_value, image=None, policy_name=None): """ Parameters @@ -166,7 +177,7 @@ def __init__(self, grasp, q_value, image=None, policy_name=None): def save(self, save_dir): """Save grasp action. - + Parameters ---------- save_dir : str @@ -185,7 +196,7 @@ def save(self, save_dir): @staticmethod def load(save_dir): """Load a saved grasp action. - + Parameters ---------- save_dir : str @@ -197,7 +208,7 @@ def load(save_dir): Loaded grasp action. """ if not os.path.exists(save_dir): - raise ValueError("Directory %s does not exist!" %(save_dir)) + raise ValueError("Directory %s does not exist!" % (save_dir)) grasp_filename = os.path.join(save_dir, "grasp.pkl") q_value_filename = os.path.join(save_dir, "pred_robustness.pkl") image_filename = os.path.join(save_dir, "tf_image.npy") @@ -207,7 +218,8 @@ def load(save_dir): if os.path.exists(image_filename): image = DepthImage.open(image_filename) return GraspAction(grasp, q_value, image) - + + class Policy(with_metaclass(ABCMeta, object)): """Abstract policy class.""" @@ -220,10 +232,11 @@ def action(self, state): """Returns an action for a given state.""" pass + class GraspingPolicy(with_metaclass(ABCMeta, Policy)): """Policy for robust grasping with Grasp Quality Convolutional Neural - Networks (GQ-CNN). - """ + Networks (GQ-CNN).""" + def __init__(self, config, init_sampler=True): """ Parameters @@ -261,39 +274,44 @@ def __init__(self, config, init_sampler=True): log_file = os.path.join(self._logging_dir, "policy.log") # Setup logger. - self._logger = Logger.get_logger(self.__class__.__name__, log_file=log_file, global_log_file=True) - + self._logger = Logger.get_logger(self.__class__.__name__, + log_file=log_file, + global_log_file=True) + # Init grasp sampler. if init_sampler: self._sampling_config = config["sampling"] self._sampling_config["gripper_width"] = self._gripper_width - if "crop_width" in config["metric"] and "crop_height" in config["metric"]: + if "crop_width" in config["metric"] and "crop_height" in config[ + "metric"]: pad = max( - math.ceil(np.sqrt(2) * (config["metric"]["crop_width"] / 2)), - math.ceil(np.sqrt(2) * (config["metric"]["crop_height"] / 2)) - ) + math.ceil( + np.sqrt(2) * (config["metric"]["crop_width"] / 2)), + math.ceil( + np.sqrt(2) * (config["metric"]["crop_height"] / 2))) self._sampling_config["min_dist_from_boundary"] = pad self._sampling_config["gripper_width"] = self._gripper_width sampler_type = self._sampling_config["type"] - self._grasp_sampler = ImageGraspSamplerFactory.sampler(sampler_type, - self._sampling_config) + self._grasp_sampler = ImageGraspSamplerFactory.sampler( + sampler_type, self._sampling_config) # Init constraint function. self._grasp_constraint_fn = None if "constraints" in self._config: self._constraint_config = self._config["constraints"] constraint_type = self._constraint_config["type"] - self._grasp_constraint_fn = GraspConstraintFnFactory.constraint_fn(constraint_type, - self._constraint_config) - + self._grasp_constraint_fn = GraspConstraintFnFactory.constraint_fn( + constraint_type, self._constraint_config) + # Init grasp quality function. self._metric_config = config["metric"] metric_type = self._metric_config["type"] - self._grasp_quality_fn = GraspQualityFunctionFactory.quality_function(metric_type, self._metric_config) + self._grasp_quality_fn = GraspQualityFunctionFactory.quality_function( + metric_type, self._metric_config) @property def config(self): - """Returns the policy configuration parameters. + """Returns the policy configuration parameters. Returns ------- @@ -304,7 +322,7 @@ def config(self): @property def grasp_sampler(self): - """Returns the grasp sampler. + """Returns the grasp sampler. Returns ------- @@ -315,7 +333,7 @@ def grasp_sampler(self): @property def grasp_quality_fn(self): - """Returns the grasp quality function. + """Returns the grasp quality function. Returns ------- @@ -326,7 +344,7 @@ def grasp_quality_fn(self): @property def grasp_constraint_fn(self): - """Returns the grasp constraint function. + """Returns the grasp constraint function. Returns ------- @@ -334,10 +352,10 @@ def grasp_constraint_fn(self): The grasp contraint function. """ return self._grasp_constraint_fn - + @property def gqcnn(self): - """Returns the GQ-CNN. + """Returns the GQ-CNN. Returns ------- @@ -354,11 +372,11 @@ def set_constraint_fn(self, constraint_fn): constraint_fn : :obj`gqcnn.grasping.constraint_fn.GraspConstraintFn` The grasp contraint function. """ - self._grasp_constraint_fn = constraint_fn - + self._grasp_constraint_fn = constraint_fn + def action(self, state): """Returns an action for a given state. - + Parameters ---------- state : :obj:`RgbdImageState` @@ -372,10 +390,12 @@ def action(self, state): # Save state. if self._logging_dir is not None: policy_id = utils.gen_experiment_id() - policy_dir = os.path.join(self._logging_dir, "policy_output_%s" % (policy_id)) + policy_dir = os.path.join(self._logging_dir, + "policy_output_%s" % (policy_id)) while os.path.exists(policy_dir): policy_id = utils.gen_experiment_id() - policy_dir = os.path.join(self._logging_dir, "policy_output_%s" % (policy_id)) + policy_dir = os.path.join(self._logging_dir, + "policy_output_%s" % (policy_id)) self._policy_dir = policy_dir os.mkdir(self._policy_dir) state_dir = os.path.join(self._policy_dir, "state") @@ -389,16 +409,16 @@ def action(self, state): action_dir = os.path.join(self._policy_dir, "action") action.save(action_dir) return action - + @abstractmethod def _action(self, state): """Returns an action for a given state. """ pass - + def show(self, filename=None, dpi=100): - """Show a figure. - + """Show a figure. + Parameters ---------- filename : str @@ -412,14 +432,16 @@ def show(self, filename=None, dpi=100): filename = os.path.join(self._policy_dir, filename) vis.savefig(filename, dpi=dpi) + class UniformRandomGraspingPolicy(GraspingPolicy): """Returns a grasp uniformly at random.""" + def __init__(self, config): - """ + """ Parameters ---------- config : dict - Python dictionary of policy configuration parameters. + Python dictionary of policy configuration parameters. filters : dict Python dictionary of functions to apply to filter invalid grasps. """ @@ -429,7 +451,7 @@ def __init__(self, config): self._grasp_center_std = 0.0 if "grasp_center_std" in config: self._grasp_center_std = config["grasp_center_std"] - + def _action(self, state): """Plans the grasp with the highest probability of success on the given RGB-D image. @@ -454,12 +476,14 @@ def _action(self, state): segmask = state.segmask # Sample grasps. - grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, - self._num_grasp_samples, - segmask=segmask, - visualize=self.config["vis"]["grasp_sampling"], - constraint_fn=self._grasp_constraint_fn, - seed=None) + grasps = self._grasp_sampler.sample( + rgbd_im, + camera_intr, + self._num_grasp_samples, + segmask=segmask, + visualize=self.config["vis"]["grasp_sampling"], + constraint_fn=self._grasp_constraint_fn, + seed=None) num_grasps = len(grasps) if num_grasps == 0: self._logger.warning("No valid grasps could be found") @@ -467,27 +491,28 @@ def _action(self, state): # Set grasp. grasp = grasps[0] - + # Perturb grasp. if self._grasp_center_std > 0.0: - grasp_center_rv = ss.multivariate_normal(grasp.center.data, cov=self._grasp_center_std**2) + grasp_center_rv = ss.multivariate_normal( + grasp.center.data, cov=self._grasp_center_std**2) grasp.center.data = grasp_center_rv.rvs(size=1)[0] - + # Form tensors. return GraspAction(grasp, 0.0, state.rgbd_im.depth) + class RobustGraspingPolicy(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. - """ + ranks the grasps by the predicted probability of success from a GQ-CNN + and returns the grasp with the highest probability of success.""" def __init__(self, config, filters=None): """ Parameters ---------- config : dict - Python dictionary of policy configuration parameters. + Python dictionary of policy configuration parameters. filters : dict Python dictionary of functions to apply to filter invalid grasps. @@ -523,11 +548,11 @@ def select(self, 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 :obj:`gqcnn.grasping.Grasp2D` or + grasps : list + Python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` grasps to select from. q_values : list Python list of associated q-values. @@ -540,12 +565,12 @@ def select(self, grasps, q_value): # Sort grasps. num_grasps = len(grasps) grasps_and_predictions = zip(np.arange(num_grasps), q_value) - grasps_and_predictions.sort(key = lambda x : x[1], reverse=True) + grasps_and_predictions.sort(key=lambda x: x[1], reverse=True) # Return top grasps. if self._filters is None: return grasps_and_predictions[0][0] - + # Filter grasps. self._logger.info("Filtering grasps") i = 0 @@ -554,8 +579,9 @@ def select(self, grasps, q_value): grasp = grasps[index] valid = True for filter_name, is_valid in self._filters.items(): - valid = is_valid(grasp) - self._logger.debug("Grasp {} filter {} valid: {}".format(i, filter_name, valid)) + valid = is_valid(grasp) + self._logger.debug("Grasp {} filter {} valid: {}".format( + i, filter_name, valid)) if not valid: valid = False break @@ -588,31 +614,38 @@ def _action(self, state): segmask = state.segmask # Sample grasps. - grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, - self._num_grasp_samples, - segmask=segmask, - visualize=self.config["vis"]["grasp_sampling"], - constraint_fn=self._grasp_constraint_fn, - seed=None) + grasps = self._grasp_sampler.sample( + rgbd_im, + camera_intr, + self._num_grasp_samples, + segmask=segmask, + visualize=self.config["vis"]["grasp_sampling"], + constraint_fn=self._grasp_constraint_fn, + seed=None) num_grasps = len(grasps) if num_grasps == 0: self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - + # Compute grasp quality. compute_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) - self._logger.debug("Grasp evaluation took %.3f sec" %(time()-compute_start)) - + self._logger.debug("Grasp evaluation took %.3f sec" % + (time() - compute_start)) + if self.config["vis"]["grasp_candidates"]: - # Display each grasp on the original image, colored by predicted success. - norm_q_values = (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) - vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) + # Display each grasp on the original image, colored by predicted + # success. + norm_q_values = (q_values - np.min(q_values)) / (np.max(q_values) - + np.min(q_values)) + vis.figure(size=(GeneralConstants.FIGSIZE, + GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config["vis"]["vmin"], vmax=self.config["vis"]["vmax"]) for grasp, q in zip(grasps, norm_q_values): - vis.grasp(grasp, scale=1.0, + vis.grasp(grasp, + scale=1.0, grasp_center_size=10, show_center=False, show_axis=True, @@ -620,7 +653,8 @@ def _action(self, state): vis.title("Sampled grasps") filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, "grasp_candidates.png") + filename = os.path.join(self._logging_dir, + "grasp_candidates.png") vis.show(filename) # Select grasp. @@ -633,13 +667,14 @@ def _action(self, state): vmin=self.config["vis"]["vmin"], vmax=self.config["vis"]["vmax"]) vis.grasp(grasp, scale=2.0, show_axis=True) - vis.title("Best Grasp: d=%.3f, q=%.3f" %(grasp.depth, q_value)) + vis.title("Best Grasp: d=%.3f, q=%.3f" % (grasp.depth, q_value)) vis.show() return GraspAction(grasp, q_value, state.rgbd_im.depth) + class CrossEntropyRobustGraspingPolicy(GraspingPolicy): - """ Optimizes a set of grasp candidates in image space using the + """Optimizes a set of grasp candidates in image space using the cross entropy method. Cross entropy method (CEM): @@ -691,7 +726,7 @@ def __init__(self, config, filters=None): self._filters = filters self._case_counter = 0 - + def _parse_config(self): """Parses the parameters of the policy.""" # Cross entropy method parameters. @@ -705,19 +740,21 @@ def _parse_config(self): self._depth_gaussian_sigma = 0.0 if "depth_gaussian_sigma" in self.config: self._depth_gaussian_sigma = self.config["depth_gaussian_sigma"] - + self._max_grasps_filter = 1 if "max_grasps_filter" in self.config: self._max_grasps_filter = self.config["max_grasps_filter"] self._max_resamples_per_iteration = 100 if "max_resamples_per_iteration" in self.config: - self._max_resamples_per_iteration = self.config["max_resamples_per_iteration"] + self._max_resamples_per_iteration = self.config[ + "max_resamples_per_iteration"] self._max_approach_angle = np.inf if "max_approach_angle" in self.config: - self._max_approach_angle = np.deg2rad(self.config["max_approach_angle"]) - + self._max_approach_angle = np.deg2rad( + self.config["max_approach_angle"]) + # Gripper parameters. self._seed = None if self.config["deterministic"]: @@ -729,18 +766,19 @@ def _parse_config(self): # Affordance map visualization. self._vis_grasp_affordance_map = False if "grasp_affordance_map" in self.config["vis"]: - self._vis_grasp_affordance_map = self.config["vis"]["grasp_affordance_map"] - - self._state_counter = 0 # Used for logging state data. + self._vis_grasp_affordance_map = self.config["vis"][ + "grasp_affordance_map"] + + self._state_counter = 0 # Used for logging state data. def select(self, grasps, q_values): - """Selects the grasp with the highest probability of success. + """Selects the grasp with the highest probability of success. Can override for alternate policies (e.g. epsilon greedy). Parameters ---------- - grasps : list + grasps : list Python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` grasps to select from. q_values : list @@ -749,20 +787,20 @@ def select(self, grasps, q_values): Returns ------- :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` - Grasp with highest probability of success. - """ + Grasp with highest probability of success. + """ # Sort. self._logger.info("Sorting grasps") num_grasps = len(grasps) if num_grasps == 0: raise NoValidGraspsException("Zero grasps") grasps_and_predictions = zip(np.arange(num_grasps), q_values) - grasps_and_predictions.sort(key = lambda x : x[1], reverse=True) + grasps_and_predictions.sort(key=lambda x: x[1], reverse=True) # Return top grasps. if self._filters is None: return grasps_and_predictions[0][0] - + # Filter grasps. self._logger.info("Filtering grasps") i = 0 @@ -771,8 +809,9 @@ def select(self, grasps, q_values): grasp = grasps[index] valid = True for filter_name, is_valid in self._filters.items(): - valid = is_valid(grasp) - self._logger.debug("Grasp {} filter {} valid: {}".format(i, filter_name, valid)) + valid = is_valid(grasp) + self._logger.debug("Grasp {} filter {} valid: {}".format( + i, filter_name, valid)) if not valid: valid = False break @@ -780,10 +819,13 @@ def select(self, grasps, q_values): return index i += 1 raise NoValidGraspsException("No grasps satisfied filters") - + def _mask_predictions(self, pred_map, segmask): self._logger.info("Masking predictions...") - assert pred_map.shape == segmask.shape, "Prediction map shape {} does not match shape of segmask {}.".format(pred_map.shape, segmask.shape) + seg_pred_mismatch_msg = ("Prediction map shape {} does not match shape" + " of segmask {}.") + assert pred_map.shape == segmask.shape, seg_pred_mismatch_msg.format( + pred_map.shape, segmask.shape) preds_masked = np.zeros_like(pred_map) nonzero_ind = np.where(segmask > 0) preds_masked[nonzero_ind] = pred_map[nonzero_ind] @@ -791,11 +833,12 @@ def _mask_predictions(self, pred_map, segmask): def _gen_grasp_affordance_map(self, state, stride=1): self._logger.info("Generating grasp affordance map...") - + # Generate grasps at points to evaluate (this is just the interface to # `GraspQualityFunction`). crop_candidate_start_time = time() - point_cloud_im = state.camera_intr.deproject_to_image(state.rgbd_im.depth) + point_cloud_im = state.camera_intr.deproject_to_image( + state.rgbd_im.depth) normal_cloud_im = point_cloud_im.normal_cloud_im() q_vals = [] @@ -803,29 +846,57 @@ def _gen_grasp_affordance_map(self, state, stride=1): gqcnn_recep_w_half = self._grasp_quality_fn.gqcnn_recep_width // 2 im_h = state.rgbd_im.height im_w = state.rgbd_im.width - for i in range(gqcnn_recep_h_half - 1, im_h - gqcnn_recep_h_half, stride): + for i in range(gqcnn_recep_h_half - 1, im_h - gqcnn_recep_h_half, + stride): grasps = [] - for j in range(gqcnn_recep_w_half - 1, im_w - gqcnn_recep_w_half, stride): - if self.config["sampling"]["type"] == "suction": # TODO(vsatish): Find a better way to find policy type. - grasps.append(SuctionPoint2D(Point(np.array([j, i])), axis=-normal_cloud_im[i, j], depth=state.rgbd_im.depth[i, j], camera_intr=state.camera_intr)) + for j in range(gqcnn_recep_w_half - 1, im_w - gqcnn_recep_w_half, + stride): + # TODO(vsatish): Find a better way to find policy type. + if self.config["sampling"]["type"] == "suction": + grasps.append( + SuctionPoint2D(Point(np.array([j, i])), + axis=-normal_cloud_im[i, j], + depth=state.rgbd_im.depth[i, j], + camera_intr=state.camera_intr)) else: - raise NotImplementedError("Parallel Jaw Grasp Affordance Maps Not Supported!") - q_vals.extend(self._grasp_quality_fn(state, grasps)) - self._logger.info("Generating crop grasp candidates took {} sec.".format(time() - crop_candidate_start_time)) - - # Mask out predictions not in the segmask (we don't really care about them). - pred_map = np.array(q_vals).reshape((im_h - gqcnn_recep_h_half * 2) // stride + 1, (im_w - gqcnn_recep_w_half * 2) // stride + 1) - tf_segmask = state.segmask.crop(im_h - gqcnn_recep_h_half * 2, im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, interp="nearest")._data.squeeze() # TODO(vsatish): Don't access the raw data like this! + raise NotImplementedError( + "Parallel Jaw Grasp Affordance Maps Not Supported!") + q_vals.extend(self._grasp_quality_fn(state, grasps)) + self._logger.info( + "Generating crop grasp candidates took {} sec.".format( + time() - crop_candidate_start_time)) + + # Mask out predictions not in the segmask (we don't really care about + # them). + pred_map = np.array(q_vals).reshape( + (im_h - gqcnn_recep_h_half * 2) // stride + 1, + (im_w - gqcnn_recep_w_half * 2) // stride + 1) + # TODO(vsatish): Don't access the raw data like this! + tf_segmask = state.segmask.crop(im_h - gqcnn_recep_h_half * 2, + im_w - gqcnn_recep_w_half * 2).resize( + 1.0 / stride, + interp="nearest")._data.squeeze() if tf_segmask.shape != pred_map.shape: new_tf_segmask = np.zeros_like(pred_map) smaller_i = min(pred_map.shape[0], tf_segmask.shape[0]) smaller_j = min(pred_map.shape[1], tf_segmask.shape[1]) - new_tf_segmask[:smaller_i, :smaller_j] = tf_segmask[:smaller_i, :smaller_j] + new_tf_segmask[:smaller_i, :smaller_j] = tf_segmask[:smaller_i, : + smaller_j] tf_segmask = new_tf_segmask pred_map_masked = self._mask_predictions(pred_map, tf_segmask) return pred_map_masked - def _plot_grasp_affordance_map(self, state, affordance_map, stride=1, grasps=None, q_values=None, plot_max=True, title=None, scale=1.0, save_fname=None, save_path=None): + def _plot_grasp_affordance_map(self, + state, + affordance_map, + stride=1, + grasps=None, + q_values=None, + plot_max=True, + title=None, + scale=1.0, + save_fname=None, + save_path=None): gqcnn_recep_h_half = self._grasp_quality_fn.gqcnn_recep_height // 2 gqcnn_recep_w_half = self._grasp_quality_fn.gqcnn_recep_width // 2 im_h = state.rgbd_im.height @@ -833,26 +904,35 @@ def _plot_grasp_affordance_map(self, state, affordance_map, stride=1, grasps=Non # Plot. vis.figure() - tf_depth_im = state.rgbd_im.depth.crop(im_h - gqcnn_recep_h_half * 2, im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, interp="nearest") + tf_depth_im = state.rgbd_im.depth.crop( + im_h - gqcnn_recep_h_half * 2, + im_w - gqcnn_recep_w_half * 2).resize(1.0 / stride, + interp="nearest") vis.imshow(tf_depth_im) - plt.imshow(affordance_map, cmap=plt.cm.RdYlGn, alpha=0.3) + plt.imshow(affordance_map, cmap=plt.cm.RdYlGn, alpha=0.3) if grasps is not None: grasps = copy.deepcopy(grasps) for grasp, q in zip(grasps, q_values): grasp.center.data[0] -= gqcnn_recep_w_half grasp.center.data[1] -= gqcnn_recep_h_half - vis.grasp(grasp, scale=scale, - show_center=False, - show_axis=True, - color=plt.cm.RdYlGn(q)) + vis.grasp(grasp, + scale=scale, + show_center=False, + show_axis=True, + color=plt.cm.RdYlGn(q)) if plot_max: - affordance_argmax = np.unravel_index(np.argmax(affordance_map), affordance_map.shape) - plt.scatter(affordance_argmax[1], affordance_argmax[0], c="black", marker=".", s=scale*25) + affordance_argmax = np.unravel_index(np.argmax(affordance_map), + affordance_map.shape) + plt.scatter(affordance_argmax[1], + affordance_argmax[0], + c="black", + marker=".", + s=scale * 25) if title is not None: vis.title(title) if save_path is not None: save_path = os.path.join(save_path, save_fname) - vis.show(save_path) + vis.show(save_path) def action_set(self, state): """Plan a set of grasps with the highest probability of success on @@ -865,7 +945,7 @@ def action_set(self, state): Returns ------- - python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` + python list of :obj:`gqcnn.grasping.Grasp2D` or :obj:`gqcnn.grasping.SuctionPoint2D` # noqa E501 Grasps to execute. """ # Check valid input. @@ -874,7 +954,9 @@ def action_set(self, state): state_output_dir = None if self._logging_dir is not None: - state_output_dir = os.path.join(self._logging_dir, "state_{}".format(str(self._state_counter).zfill(5))) + state_output_dir = os.path.join( + self._logging_dir, + "state_{}".format(str(self._state_counter).zfill(5))) if not os.path.exists(state_output_dir): os.makedirs(state_output_dir) self._state_counter += 1 @@ -887,39 +969,46 @@ def action_set(self, state): segmask = state.segmask if self._depth_gaussian_sigma > 0: - depth_im_filtered = depth_im.apply(snf.gaussian_filter, - sigma=self._depth_gaussian_sigma) + depth_im_filtered = depth_im.apply( + snf.gaussian_filter, sigma=self._depth_gaussian_sigma) else: depth_im_filtered = depth_im point_cloud_im = camera_intr.deproject_to_image(depth_im_filtered) normal_cloud_im = point_cloud_im.normal_cloud_im() - + # Visualize grasp affordance map. if self._vis_grasp_affordance_map: grasp_affordance_map = self._gen_grasp_affordance_map(state) - self._plot_grasp_affordance_map(state, grasp_affordance_map, title="Grasp Affordance Map", save_fname="affordance_map.png", save_path=state_output_dir) - - if "input_images" in self.config["vis"] and self.config["vis"]["input_images"]: + self._plot_grasp_affordance_map(state, + grasp_affordance_map, + title="Grasp Affordance Map", + save_fname="affordance_map.png", + save_path=state_output_dir) + + if "input_images" in self.config["vis"] and self.config["vis"][ + "input_images"]: vis.figure() - vis.subplot(1,2,1) + vis.subplot(1, 2, 1) vis.imshow(depth_im) vis.title("Depth") - vis.subplot(1,2,2) + vis.subplot(1, 2, 2) vis.imshow(segmask) vis.title("Segmask") filename = None if self._logging_dir is not None: filename = os.path.join(self._logging_dir, "input_images.png") vis.show(filename) - + # Sample grasps. self._logger.info("Sampling seed set") - grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, - self._num_seed_samples, - segmask=segmask, - visualize=self.config["vis"]["grasp_sampling"], - constraint_fn=self._grasp_constraint_fn, - seed=self._seed) + grasps = self._grasp_sampler.sample( + rgbd_im, + camera_intr, + self._num_seed_samples, + segmask=segmask, + visualize=self.config["vis"]["grasp_sampling"], + constraint_fn=self._grasp_constraint_fn, + seed=self._seed) num_grasps = len(grasps) if num_grasps == 0: self._logger.warning("No valid grasps could be found") @@ -931,186 +1020,245 @@ def action_set(self, state): elif isinstance(grasps[0], MultiSuctionPoint2D): grasp_type = "multi_suction" - self._logger.info("Sampled %d grasps" %(len(grasps))) - self._logger.info("Computing the seed set took %.3f sec" %(time() - seed_set_start)) + self._logger.info("Sampled %d grasps" % (len(grasps))) + self._logger.info("Computing the seed set took %.3f sec" % + (time() - seed_set_start)) # Iteratively refit and sample. for j in range(self._num_iters): - self._logger.info("CEM iter %d" %(j)) + self._logger.info("CEM iter %d" % (j)) # Predict grasps. predict_start = time() - q_values = self._grasp_quality_fn(state, grasps, params=self._config) - self._logger.info("Prediction took %.3f sec" %(time()-predict_start)) + q_values = self._grasp_quality_fn(state, + grasps, + params=self._config) + self._logger.info("Prediction took %.3f sec" % + (time() - predict_start)) # Sort grasps. resample_start = time() q_values_and_indices = zip(q_values, np.arange(num_grasps)) - q_values_and_indices.sort(key = lambda x : x[0], reverse=True) + q_values_and_indices.sort(key=lambda x: x[0], reverse=True) if self.config["vis"]["grasp_candidates"]: - # Display each grasp on the original image, colored by predicted success. - norm_q_values = q_values # (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) - title = "Sampled Grasps Iter %d" %(j) + # Display each grasp on the original image, colored by + # predicted success. + # norm_q_values = ((q_values - np.min(q_values)) / + # (np.max(q_values) - np.min(q_values))) + norm_q_values = q_values + title = "Sampled Grasps Iter %d" % (j) if self._vis_grasp_affordance_map: - self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=grasps, q_values=norm_q_values, scale=2.0, title=title, save_fname="cem_iter_{}.png".format(j), save_path=state_output_dir) + self._plot_grasp_affordance_map( + state, + grasp_affordance_map, + grasps=grasps, + q_values=norm_q_values, + scale=2.0, + title=title, + save_fname="cem_iter_{}.png".format(j), + save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) - display_grasps_and_q_values.sort(key = lambda x: x[1]) - vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) + display_grasps_and_q_values.sort(key=lambda x: x[1]) + vis.figure(size=(GeneralConstants.FIGSIZE, + GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config["vis"]["vmin"], vmax=self.config["vis"]["vmax"]) for grasp, q in display_grasps_and_q_values: - vis.grasp(grasp, scale=2.0, + vis.grasp(grasp, + scale=2.0, jaw_width=2.0, show_center=False, show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title("Sampled grasps iter %d" %(j)) + vis.title("Sampled grasps iter %d" % (j)) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, "cem_iter_%d.png" %(j)) + filename = os.path.join(self._logging_dir, + "cem_iter_%d.png" % (j)) vis.show(filename) - + # Fit elite set. elite_start = time() num_refit = max(int(np.ceil(self._gmm_refit_p * num_grasps)), 1) elite_q_values = [i[0] for i in q_values_and_indices[:num_refit]] - elite_grasp_indices = [i[1] for i in q_values_and_indices[:num_refit]] + elite_grasp_indices = [ + i[1] for i in q_values_and_indices[:num_refit] + ] elite_grasps = [grasps[i] for i in elite_grasp_indices] elite_grasp_arr = np.array([g.feature_vec for g in elite_grasps]) if self.config["vis"]["elite_grasps"]: - # Display each grasp on the original image, colored by predicted success. - norm_q_values = (elite_q_values - np.min(elite_q_values)) / (np.max(elite_q_values) - np.min(elite_q_values)) - vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) + # Display each grasp on the original image, colored by + # predicted success. + norm_q_values = (elite_q_values - np.min(elite_q_values)) / ( + np.max(elite_q_values) - np.min(elite_q_values)) + vis.figure(size=(GeneralConstants.FIGSIZE, + GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config["vis"]["vmin"], vmax=self.config["vis"]["vmax"]) for grasp, q in zip(elite_grasps, norm_q_values): - vis.grasp(grasp, scale=1.5, show_center=False, show_axis=True, + vis.grasp(grasp, + scale=1.5, + show_center=False, + show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title("Elite grasps iter %d" %(j)) + vis.title("Elite grasps iter %d" % (j)) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, "elite_set_iter_%d.png" %(j)) + filename = os.path.join(self._logging_dir, + "elite_set_iter_%d.png" % (j)) vis.show(filename) - + # Normalize elite set. elite_grasp_mean = np.mean(elite_grasp_arr, axis=0) elite_grasp_std = np.std(elite_grasp_arr, axis=0) elite_grasp_std[elite_grasp_std == 0] = 1e-6 - elite_grasp_arr = (elite_grasp_arr - elite_grasp_mean) / elite_grasp_std - self._logger.info("Elite set computation took %.3f sec" %(time()-elite_start)) + elite_grasp_arr = (elite_grasp_arr - + elite_grasp_mean) / elite_grasp_std + self._logger.info("Elite set computation took %.3f sec" % + (time() - elite_start)) # Fit a GMM to the top samples. - num_components = max(int(np.ceil(self._gmm_component_frac * num_refit)), 1) + num_components = max( + int(np.ceil(self._gmm_component_frac * num_refit)), 1) uniform_weights = (1.0 / num_components) * np.ones(num_components) gmm = GaussianMixture(n_components=num_components, weights_init=uniform_weights, reg_covar=self._gmm_reg_covar) train_start = time() gmm.fit(elite_grasp_arr) - self._logger.info("GMM fitting with %d components took %.3f sec" %(num_components, time()-train_start)) + self._logger.info("GMM fitting with %d components took %.3f sec" % + (num_components, time() - train_start)) # Sample the next grasps. grasps = [] loop_start = time() num_tries = 0 - while len(grasps) < self._num_gmm_samples and num_tries < self._max_resamples_per_iteration: + while (len(grasps) < self._num_gmm_samples + and num_tries < self._max_resamples_per_iteration): # Sample from GMM. sample_start = time() grasp_vecs, _ = gmm.sample(n_samples=self._num_gmm_samples) grasp_vecs = elite_grasp_std * grasp_vecs + elite_grasp_mean - self._logger.info("GMM sampling took %.3f sec" %(time()-sample_start)) + self._logger.info("GMM sampling took %.3f sec" % + (time() - sample_start)) # Convert features to grasps and store if in segmask. for k, grasp_vec in enumerate(grasp_vecs): feature_start = time() if grasp_type == "parallel_jaw": # Form grasp object. - grasp = Grasp2D.from_feature_vec(grasp_vec, - width=self._gripper_width, - camera_intr=camera_intr) + grasp = Grasp2D.from_feature_vec( + grasp_vec, + width=self._gripper_width, + camera_intr=camera_intr) elif grasp_type == "suction": # Read depth and approach axis. - u = int(min(max(grasp_vec[1], 0), depth_im.height-1)) - v = int(min(max(grasp_vec[0], 0), depth_im.width-1)) + u = int(min(max(grasp_vec[1], 0), depth_im.height - 1)) + v = int(min(max(grasp_vec[0], 0), depth_im.width - 1)) grasp_depth = depth_im[u, v, 0] # Approach axis. grasp_axis = -normal_cloud_im[u, v] - + # Form grasp object. - grasp = SuctionPoint2D.from_feature_vec(grasp_vec, - camera_intr=camera_intr, - depth=grasp_depth, - axis=grasp_axis) + grasp = SuctionPoint2D.from_feature_vec( + grasp_vec, + camera_intr=camera_intr, + depth=grasp_depth, + axis=grasp_axis) elif grasp_type == "multi_suction": # Read depth and approach axis. - u = int(min(max(grasp_vec[1], 0), depth_im.height-1)) - v = int(min(max(grasp_vec[0], 0), depth_im.width-1)) + u = int(min(max(grasp_vec[1], 0), depth_im.height - 1)) + v = int(min(max(grasp_vec[0], 0), depth_im.width - 1)) grasp_depth = depth_im[u, v] # Approach_axis. grasp_axis = -normal_cloud_im[u, v] - + # Form grasp object. - grasp = MultiSuctionPoint2D.from_feature_vec(grasp_vec, - camera_intr=camera_intr, - depth=grasp_depth, - axis=grasp_axis) - self._logger.debug("Feature vec took %.5f sec" %(time()-feature_start)) - + grasp = MultiSuctionPoint2D.from_feature_vec( + grasp_vec, + camera_intr=camera_intr, + depth=grasp_depth, + axis=grasp_axis) + self._logger.debug("Feature vec took %.5f sec" % + (time() - feature_start)) + bounds_start = time() # Check in bounds. - if state.segmask is None or \ - (grasp.center.y >= 0 and grasp.center.y < state.segmask.height and \ - grasp.center.x >= 0 and grasp.center.x < state.segmask.width and \ - np.any(state.segmask[int(grasp.center.y), int(grasp.center.x)] != 0) and \ - grasp.approach_angle < self._max_approach_angle) and \ - (self._grasp_constraint_fn is None or self._grasp_constraint_fn(grasp)): + if (state.segmask is None or + (grasp.center.y >= 0 + and grasp.center.y < state.segmask.height + and grasp.center.x >= 0 + and grasp.center.x < state.segmask.width + and np.any(state.segmask[int(grasp.center.y), + int(grasp.center.x)] != 0) + and grasp.approach_angle < self._max_approach_angle) + and (self._grasp_constraint_fn is None + or self._grasp_constraint_fn(grasp))): # Check validity according to filters. grasps.append(grasp) - self._logger.debug("Bounds took %.5f sec" %(time()-bounds_start)) + self._logger.debug("Bounds took %.5f sec" % + (time() - bounds_start)) num_tries += 1 - + # Check num grasps. num_grasps = len(grasps) if num_grasps == 0: self._logger.warning("No valid grasps could be found") raise NoValidGraspsException() - self._logger.info("Resample loop took %.3f sec" %(time()-loop_start)) - self._logger.info("Resampling took %.3f sec" %(time()-resample_start)) + self._logger.info("Resample loop took %.3f sec" % + (time() - loop_start)) + self._logger.info("Resampling took %.3f sec" % + (time() - resample_start)) # Predict final set of grasps. predict_start = time() q_values = self._grasp_quality_fn(state, grasps, params=self._config) - self._logger.info("Final prediction took %.3f sec" %(time()-predict_start)) + self._logger.info("Final prediction took %.3f sec" % + (time() - predict_start)) if self.config["vis"]["grasp_candidates"]: - # Display each grasp on the original image, colored by predicted success. - norm_q_values = q_values # (q_values - np.min(q_values)) / (np.max(q_values) - np.min(q_values)) + # Display each grasp on the original image, colored by predicted + # success. + # norm_q_values = ((q_values - np.min(q_values)) / + # (np.max(q_values) - np.min(q_values))) + norm_q_values = q_values title = "Final Sampled Grasps" if self._vis_grasp_affordance_map: - self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=grasps, q_values=norm_q_values, scale=2.0, title=title, save_fname="final_sampled_grasps.png".format(j), save_path=state_output_dir) + self._plot_grasp_affordance_map( + state, + grasp_affordance_map, + grasps=grasps, + q_values=norm_q_values, + scale=2.0, + title=title, + save_fname="final_sampled_grasps.png".format(j), + save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) - display_grasps_and_q_values.sort(key = lambda x: x[1]) - vis.figure(size=(GeneralConstants.FIGSIZE,GeneralConstants.FIGSIZE)) + display_grasps_and_q_values.sort(key=lambda x: x[1]) + vis.figure(size=(GeneralConstants.FIGSIZE, + GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, vmin=self.config["vis"]["vmin"], vmax=self.config["vis"]["vmax"]) for grasp, q in display_grasps_and_q_values: - vis.grasp(grasp, scale=2.0, + vis.grasp(grasp, + scale=2.0, jaw_width=2.0, show_center=False, show_axis=True, color=plt.cm.RdYlBu(q)) - vis.title("Sampled grasps iter %d" %(j)) + vis.title("Sampled grasps iter %d" % (j)) filename = None if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, "cem_iter_%d.png" %(j)) + filename = os.path.join(self._logging_dir, + "cem_iter_%d.png" % (j)) vis.show(filename) return grasps, q_values @@ -1132,8 +1280,8 @@ def _action(self, state): # Parse state. rgbd_im = state.rgbd_im depth_im = rgbd_im.depth - camera_intr = state.camera_intr - segmask = state.segmask + # camera_intr = state.camera_intr + # segmask = state.segmask # Plan grasps. grasps, q_values = self.action_set(state) @@ -1143,34 +1291,37 @@ def _action(self, state): grasp = grasps[index] q_value = q_values[index] if self.config["vis"]["grasp_plan"]: - title = "Best Grasp: d=%.3f, q=%.3f" %(grasp.depth, q_value) - if self._vis_grasp_affordance_map: - self._plot_grasp_affordance_map(state, grasp_affordance_map, grasps=[grasp], q_values=[q_value], scale=2.0, title=title, save_fname=os.path.join(case_output_dir, "best_grasp.png")) - else: - vis.figure() - vis.imshow(rgbd_im.depth, - vmin=self.config["vis"]["vmin"], - vmax=self.config["vis"]["vmax"]) - vis.grasp(grasp, scale=5.0, show_center=False, show_axis=True, jaw_width=1.0, grasp_axis_width=0.2) - vis.title(title) - filename = None - if self._logging_dir is not None: - filename = os.path.join(self._logging_dir, "planned_grasp.png") - vis.show(filename) + title = "Best Grasp: d=%.3f, q=%.3f" % (grasp.depth, q_value) + vis.figure() + vis.imshow(depth_im, + vmin=self.config["vis"]["vmin"], + vmax=self.config["vis"]["vmax"]) + vis.grasp(grasp, + scale=5.0, + show_center=False, + show_axis=True, + jaw_width=1.0, + grasp_axis_width=0.2) + vis.title(title) + filename = None + if self._logging_dir is not None: + filename = os.path.join(self._logging_dir, "planned_grasp.png") + vis.show(filename) # Form return image. - image = state.rgbd_im.depth + image = depth_im if isinstance(self._grasp_quality_fn, GQCnnQualityFunction): - image_arr, _ = self._grasp_quality_fn.grasps_to_tensors([grasp], state) - image = DepthImage(image_arr[0,...], - frame=state.rgbd_im.frame) + image_arr, _ = self._grasp_quality_fn.grasps_to_tensors([grasp], + state) + image = DepthImage(image_arr[0, ...], frame=rgbd_im.frame) # Return action. action = GraspAction(grasp, q_value, image) return action - + + class QFunctionRobustGraspingPolicy(CrossEntropyRobustGraspingPolicy): - """Optimizes a set of antipodal grasp candidates in image space using the + """Optimizes a set of antipodal grasp candidates in image space using the cross entropy method with a GQ-CNN that estimates the Q-function for use in Q-learning. @@ -1207,6 +1358,7 @@ class QFunctionRobustGraspingPolicy(CrossEntropyRobustGraspingPolicy): gripper_width : float, optional Width of the gripper in meters. """ + def __init__(self, config): CrossEntropyRobustGraspingPolicy.__init__(self, config) QFunctionRobustGraspingPolicy._parse_config(self) @@ -1233,13 +1385,14 @@ def _setup_gqcnn(self): self.gqcnn.fc5_out_size = 1 # TODO(jmahler): Implement reinitialization of pc0. - self.gqcnn.reinitialize_layers(self._reinit_fc3, - self._reinit_fc4, + self.gqcnn.reinitialize_layers(self._reinit_fc3, self._reinit_fc4, self._reinit_fc5) self.gqcnn.initialize_network(add_softmax=False) - -class EpsilonGreedyQFunctionRobustGraspingPolicy(QFunctionRobustGraspingPolicy): - """Optimizes a set of antipodal grasp candidates in image space + + +class EpsilonGreedyQFunctionRobustGraspingPolicy(QFunctionRobustGraspingPolicy + ): + """Optimizes a set of antipodal grasp candidates in image space using the cross entropy method with a GQ-CNN that estimates the Q-function for use in Q-learning, and chooses a random antipodal grasp with probability epsilon. @@ -1252,6 +1405,7 @@ class EpsilonGreedyQFunctionRobustGraspingPolicy(QFunctionRobustGraspingPolicy): ---------------- epsilon : float """ + def __init__(self, config): QFunctionRobustGraspingPolicy.__init__(self, config) self._parse_config() @@ -1283,7 +1437,7 @@ def greedy_action(self, state): Grasp to execute. """ return CrossEntropyRobustGraspingPolicy.action(self, state) - + def _action(self, state): """Plans the grasp with the highest probability of success on the given RGB-D image. @@ -1316,13 +1470,15 @@ def _action(self, state): segmask = state.segmask # Sample random antipodal grasps. - grasps = self._grasp_sampler.sample(rgbd_im, camera_intr, - self._num_seed_samples, - segmask=segmask, - visualize=self.config["vis"]["grasp_sampling"], - constraint_fn=self._grasp_constraint_fn, - seed=self._seed) - + grasps = self._grasp_sampler.sample( + rgbd_im, + camera_intr, + self._num_seed_samples, + segmask=segmask, + visualize=self.config["vis"]["grasp_sampling"], + constraint_fn=self._grasp_constraint_fn, + seed=self._seed) + num_grasps = len(grasps) if num_grasps == 0: self._logger.warning("No valid grasps could be found") @@ -1335,28 +1491,31 @@ def _action(self, state): # Create transformed image. image_tensor, pose_tensor = self.grasps_to_tensors([grasp], state) - image = DepthImage(image_tensor[0,...]) + image = DepthImage(image_tensor[0, ...]) # Predict prob success. output_arr = self.gqcnn.predict(image_tensor, pose_tensor) - q_value = output_arr[0,-1] - + q_value = output_arr[0, -1] + # Visualize planned grasp. if self.config["vis"]["grasp_plan"]: scale_factor = self.gqcnn.im_width / self._crop_width scaled_camera_intr = camera_intr.resize(scale_factor) - vis_grasp = Grasp2D(Point(image.center), 0.0, depth, + vis_grasp = Grasp2D(Point(image.center), + 0.0, + depth, width=self._gripper_width, camera_intr=scaled_camera_intr) vis.figure() vis.imshow(image) vis.grasp(vis_grasp, scale=1.5, show_center=False, show_axis=True) - vis.title("Best Grasp: d=%.3f, q=%.3f" %(depth, q_value)) + vis.title("Best Grasp: d=%.3f, q=%.3f" % (depth, q_value)) vis.show() # Return action. return GraspAction(grasp, q_value, image) + class CompositeGraspingPolicy(Policy): """Grasping policy composed of multiple sub-policies. @@ -1365,10 +1524,13 @@ class CompositeGraspingPolicy(Policy): policies : dict mapping str to `gqcnn.GraspingPolicy` Key-value dict mapping policy names to grasping policies. """ + def __init__(self, policies): - self._policies = policies - self._logger = Logger.get_logger(self.__class__.__name__, log_file=None, global_log_file=True) - + self._policies = policies + self._logger = Logger.get_logger(self.__class__.__name__, + log_file=None, + global_log_file=True) + @property def policies(self): return self._policies @@ -1379,13 +1541,15 @@ def subpolicy(self, name): def set_constraint_fn(self, constraint_fn): for policy in self._policies: policy.set_constraint_fn(constraint_fn) - + + class PriorityCompositeGraspingPolicy(CompositeGraspingPolicy): def __init__(self, policies, priority_list): # Check validity. for name in priority_list: if str(name) not in policies: - raise ValueError("Policy named %s is not in the list of policies!" %(name)) + raise ValueError( + "Policy named %s is not in the list of policies!" % (name)) self._priority_list = priority_list CompositeGraspingPolicy.__init__(self, policies) @@ -1393,7 +1557,7 @@ def __init__(self, policies, priority_list): @property def priority_list(self): return self._priority_list - + def action(self, state, policy_subset=None, min_q_value=-1.0): """Returns an action for a given state. """ @@ -1401,14 +1565,15 @@ def action(self, state, policy_subset=None, min_q_value=-1.0): i = 0 max_q = min_q_value - while action is None or (max_q <= min_q_value and i < len(self._priority_list)): + while action is None or (max_q <= min_q_value + and i < len(self._priority_list)): name = self._priority_list[i] if policy_subset is not None and name not in policy_subset: i += 1 continue self._logger.info("Planning action for sub-policy {}".format(name)) try: - action = self.policies[policy_name].action(state) + action = self.policies[name].action(state) action.policy_name = name max_q = action.q_value except NoValidGraspsException: @@ -1424,13 +1589,15 @@ def action_set(self, state, policy_subset=None, min_q_value=-1.0): actions = None q_values = None i = 0 - max_q = min_q_value - while actions is None or (max_q <= min_q_value and i < len(self._priority_list)): + max_q = min_q_value + while actions is None or (max_q <= min_q_value + and i < len(self._priority_list)): name = self._priority_list[i] if policy_subset is not None and name not in policy_subset: i += 1 continue - self._logger.info("Planning action set for sub-policy {}".format(name)) + self._logger.info( + "Planning action set for sub-policy {}".format(name)) try: actions, q_values = self.policies[name].action_set(state) for action in actions: @@ -1441,15 +1608,15 @@ def action_set(self, state, policy_subset=None, min_q_value=-1.0): i += 1 if actions is None: raise NoValidGraspsException() - return actions, q_values + return actions, q_values + class GreedyCompositeGraspingPolicy(CompositeGraspingPolicy): def __init__(self, policies): CompositeGraspingPolicy.__init__(self, policies) def action(self, state, policy_subset=None, min_q_value=-1.0): - """Returns an action for a given state. - """ + """Returns an action for a given state.""" # Compute all possible actions. actions = [] for name, policy in self.policies.items(): @@ -1459,19 +1626,18 @@ def action(self, state, policy_subset=None, min_q_value=-1.0): action = policy.action(state) action.policy_name = name actions.append() - except NoActionFoundException: + except NoValidGraspsException: pass if len(actions) == 0: raise NoValidGraspsException() - + # Rank based on q value. - actions.sort(key = lambda x: x.q_value, reverse=True) + actions.sort(key=lambda x: x.q_value, reverse=True) return actions[0] def action_set(self, state, policy_subset=None, min_q_value=-1.0): - """Returns an action for a given state. - """ + """Returns an action for a given state.""" actions = [] q_values = [] for name, policy in self.policies.items(): diff --git a/gqcnn/model/__init__.py b/gqcnn/model/__init__.py index 52fa9b96..d120daa1 100644 --- a/gqcnn/model/__init__.py +++ b/gqcnn/model/__init__.py @@ -29,13 +29,13 @@ from __future__ import division from __future__ import print_function -from .tf import * +from .tf import GQCNNTF, FCGQCNNTF from autolab_core import Logger - + + def get_gqcnn_model(backend="tf", verbose=True): - """ - Get the GQ-CNN model for the provided backend. + """Get the GQ-CNN model for the provided backend. Note: Currently only TensorFlow is supported. @@ -43,14 +43,14 @@ def get_gqcnn_model(backend="tf", verbose=True): Parameters ---------- backend : str - the backend to use, currently only "tf" is supported + The backend to use, currently only "tf" is supported. verbose : bool - whether or not to log initialization output to stdout + Whether or not to log initialization output to `stdout`. Returns ------- :obj:`gqcnn.model.tf.GQCNNTF` - GQ-CNN model with TensorFlow backend + GQ-CNN model with TensorFlow backend. """ # Set up logger. @@ -63,9 +63,9 @@ def get_gqcnn_model(backend="tf", verbose=True): else: raise ValueError("Invalid backend: {}".format(backend)) + def get_fc_gqcnn_model(backend="tf", verbose=True): - """ - Get the FC-GQ-CNN model for the provided backend. + """Get the FC-GQ-CNN model for the provided backend. Note: Currently only TensorFlow is supported. @@ -73,14 +73,14 @@ def get_fc_gqcnn_model(backend="tf", verbose=True): Parameters ---------- backend : str - the backend to use, currently only "tf" is supported + The backend to use, currently only "tf" is supported. verbose : bool - whether or not to log initialization output to stdout + Whether or not to log initialization output to `stdout`. Returns ------- :obj:`gqcnn.model.tf.FCGQCNNTF` - FC-GQ-CNN model with TensorFlow backend + FC-GQ-CNN model with TensorFlow backend. """ # Set up logger. diff --git a/gqcnn/model/tf/fc_network_tf.py b/gqcnn/model/tf/fc_network_tf.py index 2bbbe350..d48bef13 100644 --- a/gqcnn/model/tf/fc_network_tf.py +++ b/gqcnn/model/tf/fc_network_tf.py @@ -22,8 +22,11 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -FC-GQ-CNN network implemented in Tensorflow -Author: Vishal Satish +FC-GQ-CNN network implemented in Tensorflow. + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division @@ -32,19 +35,20 @@ from collections import OrderedDict import json import os -import sys import tensorflow as tf from .network_tf import GQCNNTF from ...utils import TrainingMode, InputDepthMode, GQCNNFilenames + class FCGQCNNTF(GQCNNTF): """FC-GQ-CNN network implemented in Tensorflow. - Note: - FC-GQ-CNNs are never directly trained, but instead a pre-trained GQ-CNN - is converted to an FC-GQ-CNN at inference time. + Note + ---- + FC-GQ-CNNs are never directly trained, but instead a pre-trained GQ-CNN + is converted to an FC-GQ-CNN at inference time. """ def __init__(self, gqcnn_config, fc_config, verbose=True, log_file=None): @@ -52,51 +56,60 @@ def __init__(self, gqcnn_config, fc_config, verbose=True, log_file=None): Parameters ---------- gqcnn_config : dict - python dictionary of pre-trained GQ-CNN model configuration - parameters + Python dictionary of pre-trained GQ-CNN model configuration + parameters. fc_config : dict - python dictionary of FC-GQ-CNN model configuration parameters + Python dictionary of FC-GQ-CNN model configuration parameters. verbose : bool - whether or not to log model output to stdout + Whether or not to log model output to `stdout`. log_file : str - if provided, model output will also be logged to this file + If provided, model output will also be logged to this file. """ super(FCGQCNNTF, self).__init__(gqcnn_config, log_file=log_file) - super(FCGQCNNTF, self)._parse_config(gqcnn_config) - self._parse_config(fc_config) # We call this again (even though it gets called in the parent `GQCNNTF` constructor) because the call to the parent `_parse_config` overwrites the first call. + super(FCGQCNNTF, self)._parse_config(gqcnn_config) + # We call the child `_parse_config` again (even though it gets called + # in the parent `GQCNNTF` constructor) because the call to the parent + # `_parse_config` overwrites the first call. + self._parse_config(fc_config) # Check that conv layers of GQ-CNN were trained with VALID padding. - for layer_name, layer_config in self._architecture["im_stream"].items(): + for layer_name, layer_config in self._architecture["im_stream"].items( + ): if layer_config["type"] == "conv": - assert layer_config["pad"] == "VALID", "GQ-CNN used for FC-GQ-CNN must have VALID padding for conv layers. Found layer: {} with padding: {}".format(layer_name, layer_config["pad"]) + invalid_pad_msg = ("GQ-CNN used for FC-GQ-CNN must have VALID" + " padding for conv layers. Found layer: {}" + " with padding: {}") + assert layer_config["pad"] == "VALID", invalid_pad_msg.format( + layer_name, layer_config["pad"]) @staticmethod def load(model_dir, fc_config, log_file=None): - """Load an FC-GQ-CNN from a pre-trained GQ-CNN. + """Load an FC-GQ-CNN from a pre-trained GQ-CNN. Parameters ---------- model_dir : str - path to pre-trained GQ-CNN model + Path to pre-trained GQ-CNN model. fc_config : dict - python dictionary of FC-GQ-CNN model configuration parameters + Python dictionary of FC-GQ-CNN model configuration parameters. log_file : str - if provided, model output will also be logged to this file + If provided, model output will also be logged to this file. Returns ------- :obj:`FCGQCNNTF` - initialized FC-GQ-CNN + Initialized FC-GQ-CNN. """ config_file = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) - with open(config_file) as data_file: + with open(config_file) as data_file: train_config = json.load(data_file, object_pairs_hook=OrderedDict) gqcnn_config = train_config["gqcnn"] - + # Initialize weights and Tensorflow network. fcgqcnn = FCGQCNNTF(gqcnn_config, fc_config, log_file=log_file) - fcgqcnn.init_weights_file(os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL)) + fcgqcnn.init_weights_file( + os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL)) fcgqcnn.init_mean_and_std(model_dir) training_mode = train_config["training_mode"] if training_mode == TrainingMode.CLASSIFICATION: @@ -116,35 +129,58 @@ def _pack(self, dim_h, dim_w, data, vector=False): if vector: # First reshape vector into 3-dimensional tensor. reshaped = tf.reshape(data, tf.concat([[1, 1], tf.shape(data)], 0)) - + # Then tile into tensor of shape dim_h x dim_w x `data.dim0`. packed = tf.tile(reshaped, [dim_h, dim_w, 1]) else: - # First reshape second dimension of tensor into 3-dimensional tensor. - reshaped = tf.reshape(data, tf.concat([tf.shape(data)[0:1], [1, 1], tf.shape(data)[1:]], 0)) - - # Then tile into tensor of shape bsize=1 x dim_h x dim_w x `data.dim1`. + # First reshape second dimension of tensor into 3-dimensional + # tensor. + reshaped = tf.reshape( + data, + tf.concat([tf.shape(data)[0:1], [1, 1], + tf.shape(data)[1:]], 0)) + + # Then tile into tensor of shape + # bsize=1 x dim_h x dim_w x `data.dim1`. packed = tf.tile(reshaped, [1, dim_h, dim_w, 1]) return packed - def _build_fully_conv_layer(self, input_node, filter_dim, fc_name, final_fc_layer=False): - self._logger.info("Converting fc layer {} to fully convolutional...".format(fc_name)) - + def _build_fully_conv_layer(self, + input_node, + filter_dim, + fc_name, + final_fc_layer=False): + self._logger.info( + "Converting fc layer {} to fully convolutional...".format(fc_name)) + # Create new set of weights by reshaping fully connected layer weights. fcW = self._weights.weights["{}_weights".format(fc_name)] - convW = tf.Variable(tf.reshape(fcW, tf.concat([[filter_dim, filter_dim], [tf.shape(fcW)[0] // (filter_dim * filter_dim)], tf.shape(fcW)[1:]], 0)), name="{}_fully_conv_weights".format(fc_name)) + convW = tf.Variable(tf.reshape( + fcW, + tf.concat([[filter_dim, filter_dim], + [tf.shape(fcW)[0] // (filter_dim * filter_dim)], + tf.shape(fcW)[1:]], 0)), + name="{}_fully_conv_weights".format(fc_name)) self._weights.weights["{}_fully_conv_weights".format(fc_name)] = convW - + # Get bias. convb = self._weights.weights["{}_bias".format(fc_name)] - # Compute conv out (note that we use padding="VALID" here because we want an output size of 1 x 1 x num_filts for the original input size). - convh = tf.nn.conv2d(input_node, convW, strides=[1, 1, 1, 1], padding="VALID") + # Compute conv out (note that we use padding="VALID" here because we + # want an output size of 1 x 1 x num_filts for the original input + # size). + convh = tf.nn.conv2d(input_node, + convW, + strides=[1, 1, 1, 1], + padding="VALID") # Pack bias into tensor of shape `tf.shape(convh)`. # TODO(vsatish): This is unnecessary and can be removed for potential # performance improvements. - bias_packed = self._pack(tf.shape(convh)[1], tf.shape(convh)[2], convb, vector=True) + bias_packed = self._pack(tf.shape(convh)[1], + tf.shape(convh)[2], + convb, + vector=True) # Add bias term. convh = convh + bias_packed @@ -158,108 +194,176 @@ def _build_fully_conv_layer(self, input_node, filter_dim, fc_name, final_fc_laye return convh - def _build_fully_conv_merge_layer(self, input_node_im, input_node_pose, filter_dim, fc_name): - self._logger.info("Converting fc merge layer {} to fully convolutional...".format(fc_name)) + def _build_fully_conv_merge_layer(self, input_node_im, input_node_pose, + filter_dim, fc_name): + self._logger.info( + "Converting fc merge layer {} to fully convolutional...".format( + fc_name)) - # Create new set of weights for image stream by reshaping fully-connected layer weights. + # Create new set of weights for image stream by reshaping + # fully-connected layer weights. fcW_im = self._weights.weights["{}_input_1_weights".format(fc_name)] - convW = tf.Variable(tf.reshape(fcW_im, tf.concat([[filter_dim, filter_dim], [tf.shape(fcW_im)[0] // (filter_dim * filter_dim)], tf.shape(fcW_im)[1:]], 0)), name="{}_im_fully_conv_weights".format(fc_name)) - self._weights.weights["{}_im_fully_conv_weights".format(fc_name)] = convW - - # Compute im stream conv out (note that we use padding="VALID" here because we want an output size of 1 x 1 x num_filts for the original input size). - convh_im = tf.nn.conv2d(input_node_im, convW, strides=[1, 1, 1, 1], padding="VALID") + convW = tf.Variable(tf.reshape( + fcW_im, + tf.concat([[filter_dim, filter_dim], + [tf.shape(fcW_im)[0] // (filter_dim * filter_dim)], + tf.shape(fcW_im)[1:]], 0)), + name="{}_im_fully_conv_weights".format(fc_name)) + self._weights.weights["{}_im_fully_conv_weights".format( + fc_name)] = convW + + # Compute im stream conv out (note that we use padding="VALID" here + # because we want an output size of 1 x 1 x num_filts for the original + # input size). + convh_im = tf.nn.conv2d(input_node_im, + convW, + strides=[1, 1, 1, 1], + padding="VALID") # Get pose stream fully-connected weights. fcW_pose = self._weights.weights["{}_input_2_weights".format(fc_name)] - # Compute matmul for pose stream. + # Compute matmul for pose stream. pose_out = tf.matmul(input_node_pose, fcW_pose) # Pack pose_out into a tensor of shape=tf.shape(convh_im). # TODO(vsatish): This is unnecessary and can be removed for potential - # performance improvements. - pose_packed = self._pack(tf.shape(convh_im)[1], tf.shape(convh_im)[2], pose_out) + # performance improvements. + pose_packed = self._pack( + tf.shape(convh_im)[1], + tf.shape(convh_im)[2], pose_out) - # Add the im and pose tensors. + # Add the im and pose tensors. convh = convh_im + pose_packed # Pack bias. # TODO(vsatish): This is unnecessary and can be removed for potential # performance improvements. fc_bias = self._weights.weights["{}_bias".format(fc_name)] - bias_packed = self._pack(tf.shape(convh_im)[1], tf.shape(convh_im)[2], fc_bias, vector=True) + bias_packed = self._pack(tf.shape(convh_im)[1], + tf.shape(convh_im)[2], + fc_bias, + vector=True) # Add bias and apply activation. convh = self._leaky_relu(convh + bias_packed, alpha=self._relu_coeff) return convh - def _build_im_stream(self, input_node, input_pose_node, input_height, input_width, input_channels, drop_rate, layers, only_stream=False): + def _build_im_stream(self, + input_node, + input_pose_node, + input_height, + input_width, + input_channels, + drop_rate, + layers, + only_stream=False): self._logger.info("Building Image Stream...") if self._input_depth_mode == InputDepthMode.SUB: sub_mean = tf.constant(self._im_depth_sub_mean, dtype=tf.float32) sub_std = tf.constant(self._im_depth_sub_std, dtype=tf.float32) - sub_im = tf.subtract(input_node, tf.tile(tf.reshape(input_pose_node, tf.constant((-1, 1, 1, 1))), tf.constant((1, input_height, input_width, 1)))) + sub_im = tf.subtract( + input_node, + tf.tile( + tf.reshape(input_pose_node, tf.constant((-1, 1, 1, 1))), + tf.constant((1, input_height, input_width, 1)))) norm_sub_im = tf.div(tf.subtract(sub_im, sub_mean), sub_std) input_node = norm_sub_im output_node = input_node - prev_layer = "start" # Dummy placeholder. + prev_layer = "start" # Dummy placeholder. filter_dim = self._train_im_width last_index = len(layers) - 1 - for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + for layer_index, (layer_name, + layer_config) in enumerate(layers.items()): layer_type = layer_config["type"] if layer_type == "conv": if prev_layer == "fc": raise ValueError("Cannot have conv layer after fc layer!") - output_node, input_height, input_width, input_channels = self._build_conv_layer(output_node, input_height, input_width, input_channels, layer_config["filt_dim"], layer_config["filt_dim"], layer_config["num_filt"], layer_config["pool_stride"], layer_config["pool_stride"], layer_config["pool_size"], layer_name, norm=layer_config["norm"], pad=layer_config["pad"]) + output_node, input_height, input_width, input_channels = \ + self._build_conv_layer( + output_node, + input_height, + input_width, + input_channels, + layer_config["filt_dim"], + layer_config["filt_dim"], + layer_config["num_filt"], + layer_config["pool_stride"], + layer_config["pool_stride"], + layer_config["pool_size"], + layer_name, + norm=layer_config["norm"], + pad=layer_config["pad"]) prev_layer = layer_type if layer_config["pad"] == "SAME": filter_dim //= layer_config["pool_stride"] else: - filter_dim = ((filter_dim - layer_config["filt_dim"]) // layer_config["pool_stride"]) + 1 + filter_dim = ((filter_dim - layer_config["filt_dim"]) // + layer_config["pool_stride"]) + 1 elif layer_type == "fc": if layer_index == last_index and only_stream: - output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name, final_fc_layer=True) + output_node = self._build_fully_conv_layer( + output_node, + filter_dim, + layer_name, + final_fc_layer=True) else: - output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name) + output_node = self._build_fully_conv_layer( + output_node, filter_dim, layer_name) prev_layer = layer_type - filter_dim = 1 # Because fully-convolutional layers at this point in the network have a filter_dim of 1. + # Because fully-convolutional layers at this point in the + # network have a filter_dim of 1. + filter_dim = 1 elif layer_type == "pc": - raise ValueError("Cannot have pose connected layer in image stream!") + raise ValueError( + "Cannot have pose connected layer in image stream!") elif layer_type == "fc_merge": raise ValueError("Cannot have merge layer in image stream!") else: - raise ValueError("Unsupported layer type: {}".format(layer_type)) + raise ValueError( + "Unsupported layer type: {}".format(layer_type)) return output_node, -1 - def _build_merge_stream(self, input_stream_1, input_stream_2, fan_in_1, fan_in_2, drop_rate, layers): + def _build_merge_stream(self, input_stream_1, input_stream_2, fan_in_1, + fan_in_2, drop_rate, layers): self._logger.info("Building Merge Stream...") - + # First check if first layer is a merge layer. if layers[list(layers)[0]]["type"] != "fc_merge": - raise ValueError("First layer in merge stream must be of type fc_merge!") - - prev_layer = "start" # Dummy placeholder. + raise ValueError( + "First layer in merge stream must be of type fc_merge!") + last_index = len(layers) - 1 - filter_dim = 1 # Because fully-convolutional layers at this point in the network have a filter_dim of 1. + # Because fully-convolutional layers at this point in the network have + # a filter_dim of 1. + filter_dim = 1 fan_in = -1 - for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + output_node = None # Will be overridden. + for layer_index, (layer_name, + layer_config) in enumerate(layers.items()): layer_type = layer_config["type"] if layer_type == "conv": - raise ValueError("Cannot have conv layer in merge stream!") + raise ValueError("Cannot have conv layer in merge stream!") elif layer_type == "fc": if layer_index == last_index: - output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name, final_fc_layer=True) + output_node = self._build_fully_conv_layer( + output_node, + filter_dim, + layer_name, + final_fc_layer=True) else: - output_node = self._build_fully_conv_layer(output_node, filter_dim, layer_name) - prev_layer = layer_type - elif layer_type == "pc": - raise ValueError("Cannot have pose connected layer in merge stream!") + output_node = self._build_fully_conv_layer( + output_node, filter_dim, layer_name) + elif layer_type == "pc": + raise ValueError( + "Cannot have pose connected layer in merge stream!") elif layer_type == "fc_merge": - output_node = self._build_fully_conv_merge_layer(input_stream_1, input_stream_2, filter_dim, layer_name) - prev_layer = layer_type + output_node = self._build_fully_conv_merge_layer( + input_stream_1, input_stream_2, filter_dim, layer_name) else: - raise ValueError("Unsupported layer type: {}".format(layer_type)) + raise ValueError( + "Unsupported layer type: {}".format(layer_type)) return output_node, fan_in diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index 87fe93c4..a343e929 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -23,18 +23,21 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. GQ-CNN network implemented in Tensorflow. -Authors: Vishal Satish, Jeff Mahler + +Author +------ +Vishal Satish & Jeff Mahler """ from __future__ import absolute_import from __future__ import division from __future__ import print_function from collections import OrderedDict +from functools import reduce import json import math import operator import os -import sys import time import numpy as np @@ -42,15 +45,18 @@ import tensorflow.contrib.framework as tcf from autolab_core import Logger -from ...utils import (reduce_shape, read_pose_data, pose_dim, - weight_name_to_layer_name, GripperMode, - TrainingMode, InputDepthMode, GQCNNFilenames) +from ...utils import (reduce_shape, read_pose_data, pose_dim, + weight_name_to_layer_name, GripperMode, TrainingMode, + InputDepthMode, GQCNNFilenames) + class GQCNNWeights(object): """Helper struct for storing network weights.""" + def __init__(self): self.weights = {} + class GQCNNTF(object): """GQ-CNN network implemented in Tensorflow.""" @@ -59,131 +65,156 @@ def __init__(self, gqcnn_config, verbose=True, log_file=None): Parameters ---------- gqcnn_config : dict - python dictionary of model configuration parameters + Python dictionary of model configuration parameters. verbose : bool - whether or not to log model output to stdout + Whether or not to log model output to `stdout`. log_file : str - if provided, model output will also be logged to this file + If provided, model output will also be logged to this file. """ self._sess = None self._graph = tf.Graph() # Set up logger. - self._logger = Logger.get_logger(self.__class__.__name__, log_file=log_file, silence=(not verbose), global_log_file=verbose) - + self._logger = Logger.get_logger(self.__class__.__name__, + log_file=log_file, + silence=(not verbose), + global_log_file=verbose) + self._weights = GQCNNWeights() self._parse_config(gqcnn_config) @staticmethod def load(model_dir, verbose=True, log_file=None): - """Instantiate a trained GQ-CNN for fine-tuning or inference. + """Instantiate a trained GQ-CNN for fine-tuning or inference. Parameters ---------- model_dir : str - path to trained GQ-CNN model + Path to trained GQ-CNN model. verbose : bool - whether or not to log model output to stdout + Whether or not to log model output to `stdout`. log_file : str - if provided, model output will also be logged to this file - + If provided, model output will also be logged to this file. + Returns ------- :obj:`GQCNNTF` - initialized GQ-CNN + Initialized GQ-CNN. """ config_file = os.path.join(model_dir, GQCNNFilenames.SAVED_CFG) - with open(config_file) as data_file: + with open(config_file) as data_file: train_config = json.load(data_file, object_pairs_hook=OrderedDict) # Support for legacy configs. try: gqcnn_config = train_config["gqcnn"] - except: - gqcnn_config = train_config["gqcnn_config"] + except KeyError: + gqcnn_config = train_config["gqcnn_config"] # Convert old networks to new flexible arch format. gqcnn_config["debug"] = 0 gqcnn_config["seed"] = 0 - gqcnn_config["num_angular_bins"] = 0 # Legacy networks had no angular support. - gqcnn_config["input_depth_mode"] = InputDepthMode.POSE_STREAM # Legacy networks only supported depth integration through pose stream. + # Legacy networks had no angular support. + gqcnn_config["num_angular_bins"] = 0 + # Legacy networks only supported depth integration through pose + # stream. + gqcnn_config["input_depth_mode"] = InputDepthMode.POSE_STREAM arch_config = gqcnn_config["architecture"] if "im_stream" not in arch_config: new_arch_config = OrderedDict() new_arch_config["im_stream"] = OrderedDict() new_arch_config["pose_stream"] = OrderedDict() - new_arch_config["merge_stream"] = OrderedDict() + new_arch_config["merge_stream"] = OrderedDict() layer_name = "conv1_1" - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["im_stream"][layer_name]["type"] = "conv" new_arch_config["im_stream"][layer_name]["pad"] = "SAME" if "padding" in arch_config[layer_name]: - new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + new_arch_config["im_stream"][layer_name][ + "pad"] = arch_config[layer_name]["padding"] layer_name = "conv1_2" - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] - new_arch_config["im_stream"][layer_name]["type"] = "conv" + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "conv" new_arch_config["im_stream"][layer_name]["pad"] = "SAME" if "padding" in arch_config[layer_name]: - new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + new_arch_config["im_stream"][layer_name][ + "pad"] = arch_config[layer_name]["padding"] layer_name = "conv2_1" - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["im_stream"][layer_name]["type"] = "conv" new_arch_config["im_stream"][layer_name]["pad"] = "SAME" if "padding" in arch_config[layer_name]: - new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + new_arch_config["im_stream"][layer_name][ + "pad"] = arch_config[layer_name]["padding"] layer_name = "conv2_2" - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["im_stream"][layer_name]["type"] = "conv" new_arch_config["im_stream"][layer_name]["pad"] = "SAME" if "padding" in arch_config[layer_name]: - new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + new_arch_config["im_stream"][layer_name][ + "pad"] = arch_config[layer_name]["padding"] layer_name = "conv3_1" if layer_name in arch_config: - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["im_stream"][layer_name]["type"] = "conv" new_arch_config["im_stream"][layer_name]["pad"] = "SAME" if "padding" in arch_config[layer_name]: - new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + new_arch_config["im_stream"][layer_name][ + "pad"] = arch_config[layer_name]["padding"] layer_name = "conv3_2" if layer_name in arch_config: - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["im_stream"][layer_name]["type"] = "conv" new_arch_config["im_stream"][layer_name]["pad"] = "SAME" if "padding" in arch_config[layer_name]: - new_arch_config["im_stream"][layer_name]["pad"] = arch_config[layer_name]["padding"] + new_arch_config["im_stream"][layer_name][ + "pad"] = arch_config[layer_name]["padding"] layer_name = "fc3" - new_arch_config["im_stream"][layer_name] = arch_config[layer_name] - new_arch_config["im_stream"][layer_name]["type"] = "fc" - + new_arch_config["im_stream"][layer_name] = arch_config[ + layer_name] + new_arch_config["im_stream"][layer_name]["type"] = "fc" + layer_name = "pc1" - new_arch_config["pose_stream"][layer_name] = arch_config[layer_name] + new_arch_config["pose_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["pose_stream"][layer_name]["type"] = "pc" layer_name = "pc2" if layer_name in arch_config: - new_arch_config["pose_stream"][layer_name] = arch_config[layer_name] + new_arch_config["pose_stream"][layer_name] = arch_config[ + layer_name] new_arch_config["pose_stream"][layer_name]["type"] = "pc" - + layer_name = "fc4" - new_arch_config["merge_stream"][layer_name] = arch_config[layer_name] - new_arch_config["merge_stream"][layer_name]["type"] = "fc_merge" + new_arch_config["merge_stream"][layer_name] = arch_config[ + layer_name] + new_arch_config["merge_stream"][layer_name][ + "type"] = "fc_merge" layer_name = "fc5" - new_arch_config["merge_stream"][layer_name] = arch_config[layer_name] - new_arch_config["merge_stream"][layer_name]["type"] = "fc" + new_arch_config["merge_stream"][layer_name] = arch_config[ + layer_name] + new_arch_config["merge_stream"][layer_name]["type"] = "fc" gqcnn_config["architecture"] = new_arch_config - + # Initialize weights and Tensorflow network. gqcnn = GQCNNTF(gqcnn_config, verbose=verbose, log_file=log_file) - gqcnn.init_weights_file(os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL)) + gqcnn.init_weights_file( + os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL)) gqcnn.init_mean_and_std(model_dir) training_mode = train_config["training_mode"] if training_mode == TrainingMode.CLASSIFICATION: @@ -196,64 +227,82 @@ def load(model_dir, verbose=True, log_file=None): def init_mean_and_std(self, model_dir): """Loads the means and stds of a trained GQ-CNN to use for data - normalization during inference. + normalization during inference. Parameters ---------- model_dir : str - path to trained GQ-CNN model where means and standard deviations - are stored + Path to trained GQ-CNN model where means and standard deviations + are stored. """ - # Load in means and stds. + # Load in means and stds. if self._input_depth_mode == InputDepthMode.POSE_STREAM: try: - self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames.IM_MEAN)) - self._im_std = np.load(os.path.join(model_dir, GQCNNFilenames.IM_STD)) - except: + self._im_mean = np.load( + os.path.join(model_dir, GQCNNFilenames.IM_MEAN)) + self._im_std = np.load( + os.path.join(model_dir, GQCNNFilenames.IM_STD)) + except FileNotFoundError: # Support for legacy file naming convention. - self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames.LEG_MEAN)) - self._im_std = np.load(os.path.join(model_dir, GQCNNFilenames.LEG_STD)) - self._pose_mean = np.load(os.path.join(model_dir, GQCNNFilenames.POSE_MEAN)) - self._pose_std = np.load(os.path.join(model_dir, GQCNNFilenames.POSE_STD)) + self._im_mean = np.load( + os.path.join(model_dir, GQCNNFilenames.LEG_MEAN)) + self._im_std = np.load( + os.path.join(model_dir, GQCNNFilenames.LEG_STD)) + self._pose_mean = np.load( + os.path.join(model_dir, GQCNNFilenames.POSE_MEAN)) + self._pose_std = np.load( + os.path.join(model_dir, GQCNNFilenames.POSE_STD)) # Read the certain parts of the pose mean/std that we desire. - if len(self._pose_mean.shape) > 0 and self._pose_mean.shape[0] != self._pose_dim: + if len(self._pose_mean.shape + ) > 0 and self._pose_mean.shape[0] != self._pose_dim: # Handle multi-dim storage. - if len(self._pose_mean.shape) > 1 and self._pose_mean.shape[1] == self._pose_dim: - self._pose_mean = self._pose_mean[0,:] - self._pose_std = self._pose_std[0,:] + if len(self._pose_mean.shape + ) > 1 and self._pose_mean.shape[1] == self._pose_dim: + self._pose_mean = self._pose_mean[0, :] + self._pose_std = self._pose_std[0, :] else: - self._pose_mean = read_pose_data(self._pose_mean, self._gripper_mode) - self._pose_std = read_pose_data(self._pose_std, self._gripper_mode) + self._pose_mean = read_pose_data(self._pose_mean, + self._gripper_mode) + self._pose_std = read_pose_data(self._pose_std, + self._gripper_mode) elif self._input_depth_mode == InputDepthMode.SUB: - self._im_depth_sub_mean = np.load(os.path.join(model_dir, GQCNNFilenames.IM_DEPTH_SUB_MEAN)) - self._im_depth_sub_std = np.load(os.path.join(model_dir, GQCNNFilenames.IM_DEPTH_SUB_STD)) + self._im_depth_sub_mean = np.load( + os.path.join(model_dir, GQCNNFilenames.IM_DEPTH_SUB_MEAN)) + self._im_depth_sub_std = np.load( + os.path.join(model_dir, GQCNNFilenames.IM_DEPTH_SUB_STD)) elif self._input_depth_mode == InputDepthMode.IM_ONLY: - self._im_mean = np.load(os.path.join(model_dir, GQCNNFilenames.IM_MEAN)) - self._im_std = np.load(os.path.join(model_dir, GQCNNFilenames.IM_STD)) + self._im_mean = np.load( + os.path.join(model_dir, GQCNNFilenames.IM_MEAN)) + self._im_std = np.load( + os.path.join(model_dir, GQCNNFilenames.IM_STD)) else: - raise ValueError("Unsupported input depth mode: {}".format(self._input_depth_mode)) - + raise ValueError("Unsupported input depth mode: {}".format( + self._input_depth_mode)) + def set_base_network(self, model_dir): """Initialize network weights for the base network. Used during - fine-tuning. + fine-tuning. Parameters ---------- model_dir : str - path to pre-trained GQ-CNN model + Path to pre-trained GQ-CNN model. """ # Check architecture. if "base_model" not in self._architecture: - self._logger.warning("Architecuture has no base model. The network has not been modified.") + self._logger.warning( + "Architecuture has no base model. The network has not been" + " modified.") return False base_model_config = self._architecture["base_model"] output_layer = base_model_config["output_layer"] - + # Read model. ckpt_file = os.path.join(model_dir, GQCNNFilenames.FINAL_MODEL) config_file = os.path.join(model_dir, GQCNNFilenames.SAVED_ARCH) - base_arch = json.load(open(config_file, "r"), object_pairs_hook=OrderedDict) + base_arch = json.load(open(config_file, "r"), + object_pairs_hook=OrderedDict) # Read base layer names. self._base_layer_names = [] @@ -281,11 +330,11 @@ def set_base_network(self, model_dir): found_base_layer = True except StopIteration: stop = True - + with self._graph.as_default(): # Create new tf checkpoint reader. reader = tf.train.NewCheckpointReader(ckpt_file) - + # Create empty weights object. self._weights = GQCNNWeights() @@ -304,20 +353,21 @@ def set_base_network(self, model_dir): # Add weights. if layer_name in self._base_layer_names: - self._weights.weights[short_name] = tf.Variable(reader.get_tensor(full_var_name), name=full_var_name) - + self._weights.weights[short_name] = tf.Variable( + reader.get_tensor(full_var_name), name=full_var_name) + def init_weights_file(self, ckpt_file): - """Load trained GQ-CNN weights. + """Load trained GQ-CNN weights. Parameters ---------- ckpt_file : str - Tensorflow checkpoint file from which to load model weights + Tensorflow checkpoint file from which to load model weights. """ with self._graph.as_default(): # Create new tf checkpoint reader. reader = tf.train.NewCheckpointReader(ckpt_file) - + # Create empty weight object. self._weights = GQCNNWeights() @@ -328,10 +378,11 @@ def init_weights_file(self, ckpt_file): for variable, shape in ckpt_vars: full_var_names.append(variable) short_names.append(variable.split("/")[-1]) - + # Load variables. for full_var_name, short_name in zip(full_var_names, short_names): - self._weights.weights[short_name] = tf.Variable(reader.get_tensor(full_var_name), name=full_var_name) + self._weights.weights[short_name] = tf.Variable( + reader.get_tensor(full_var_name), name=full_var_name) def _parse_config(self, gqcnn_config): """Parse configuration file. @@ -339,10 +390,10 @@ def _parse_config(self, gqcnn_config): Parameters ---------- gqcnn_config : dict - python dictionary of model configuration parameters + Python dictionary of model configuration parameters. """ - - ##################### PARSING GQCNN CONFIG ##################### + + # Parse GQ-CNN config. # Load tensor params. self._batch_size = gqcnn_config["batch_size"] self._train_im_height = gqcnn_config["im_height"] @@ -352,23 +403,27 @@ def _parse_config(self, gqcnn_config): self._num_channels = gqcnn_config["im_channels"] try: self._gripper_mode = gqcnn_config["gripper_mode"] - except: + except KeyError: # Legacy support. self._input_data_mode = gqcnn_config["input_data_mode"] if self._input_data_mode == "tf_image": self._gripper_mode = GripperMode.LEGACY_PARALLEL_JAW elif self._input_data_mode == "tf_image_suction": - self._gripper_mode = GripperMode.LEGACY_SUCTION + self._gripper_mode = GripperMode.LEGACY_SUCTION elif self._input_data_mode == "suction": self._gripper_mode = GripperMode.SUCTION elif self._input_data_mode == "multi_suction": - self._gripper_mode = GripperMode.MULTI_SUCTION + self._gripper_mode = GripperMode.MULTI_SUCTION elif self._input_data_mode == "parallel_jaw": self._gripper_mode = GripperMode.PARALLEL_JAW else: - raise ValueError("Legacy input data mode: {} not supported!".format(self._input_data_mode)) - self._logger.warning("Could not read gripper mode. Attempting legacy conversion to: {}".format(self._gripper_mode)) - + raise ValueError( + "Legacy input data mode: {} not supported!".format( + self._input_data_mode)) + self._logger.warning("Could not read gripper mode. Attempting" + " legacy conversion to: {}".format( + self._gripper_mode)) + # Setup gripper pose dimension depending on gripper mode. self._pose_dim = pose_dim(self._gripper_mode) @@ -376,10 +431,10 @@ def _parse_config(self, gqcnn_config): self._architecture = gqcnn_config["architecture"] # Get input depth mode. - self._input_depth_mode = InputDepthMode.POSE_STREAM # Legacy support. + self._input_depth_mode = InputDepthMode.POSE_STREAM # Legacy support. if "input_depth_mode" in gqcnn_config: self._input_depth_mode = gqcnn_config["input_depth_mode"] - + # Load network local response normalization layer constants. self._normalization_radius = gqcnn_config["radius"] self._normalization_alpha = gqcnn_config["alpha"] @@ -387,7 +442,7 @@ def _parse_config(self, gqcnn_config): self._normalization_bias = gqcnn_config["bias"] # Get ReLU coefficient. - self._relu_coeff = 0.0 # Legacy support. + self._relu_coeff = 0.0 # Legacy support. if "relu_coeff" in gqcnn_config: self._relu_coeff = gqcnn_config["relu_coeff"] @@ -409,7 +464,7 @@ def _parse_config(self, gqcnn_config): self._im_std = 1 # Get number of angular bins. - self._angular_bins = 0 # Legacy support. + self._angular_bins = 0 # Legacy support. if "angular_bins" in gqcnn_config: self._angular_bins = gqcnn_config["angular_bins"] @@ -417,30 +472,41 @@ def _parse_config(self, gqcnn_config): self._max_angle = np.pi if "max_angle" in gqcnn_config: self._max_angle = np.deg2rad(gqcnn_config["max_angle"]) - - # If using angular bins, make sure output size of final fully connected layer is 2x number of angular bins (because of failure/success probs for each bin). + + # If using angular bins, make sure output size of final fully connected + # layer is 2x number of angular bins (because of failure/success probs + # for each bin). if self._angular_bins > 0: - assert list(list(self._architecture.values())[-1].values())[-1]["out_size"] == 2 * self._angular_bins, "When predicting angular outputs, output size of final fully connected layer must be 2x number of angular bins" + final_out_size = list( + list(self._architecture.values())[-1].values())[-1]["out_size"] + ang_mismatch_msg = ("When predicting angular outputs, output" + " size of final fully connected layer must" + " be 2x number of angular bins.") + assert final_out_size == 2 * self._angular_bins, ang_mismatch_msg # Intermediate network feature handles. self._feature_tensors = {} - + # Base layer names for fine-tuning. self._base_layer_names = [] - - def initialize_network(self, train_im_node=None, train_pose_node=None, add_softmax=False, add_sigmoid=False): + + def initialize_network(self, + train_im_node=None, + train_pose_node=None, + add_softmax=False, + add_sigmoid=False): """Set up input placeholders and build network. Parameters ---------- train_im_node : :obj:`tf.placeholder` - images for training + Images for training. train_pose_node : :obj:`tf.placeholder` - poses for training + Poses for training. add_softmax : bool - whether or not to add a softmax layer to output of network + Whether or not to add a softmax layer to output of network. add_sigmoid : bool - whether or not to add a sigmoid layer to output of network + Whether or not to add a sigmoid layer to output of network. """ with self._graph.as_default(): # Set TF random seed if debugging. @@ -450,26 +516,40 @@ def initialize_network(self, train_im_node=None, train_pose_node=None, add_softm # Setup input placeholders. if train_im_node is not None: # Training. - self._input_im_node = tf.placeholder_with_default(train_im_node, (None, self._im_height, self._im_width, self._num_channels)) - self._input_pose_node = tf.placeholder_with_default(train_pose_node, (None, self._pose_dim)) + self._input_im_node = tf.placeholder_with_default( + train_im_node, (None, self._im_height, self._im_width, + self._num_channels)) + self._input_pose_node = tf.placeholder_with_default( + train_pose_node, (None, self._pose_dim)) else: # Inference only using GQ-CNN instantiated from `GQCNNTF.load`. - self._input_im_node = tf.placeholder(tf.float32, (self._batch_size, self._im_height, self._im_width, self._num_channels)) - self._input_pose_node = tf.placeholder(tf.float32, (self._batch_size, self._pose_dim)) - self._input_drop_rate_node = tf.placeholder_with_default(tf.constant(0.0), ()) + self._input_im_node = tf.placeholder( + tf.float32, (self._batch_size, self._im_height, + self._im_width, self._num_channels)) + self._input_pose_node = tf.placeholder( + tf.float32, (self._batch_size, self._pose_dim)) + self._input_drop_rate_node = tf.placeholder_with_default( + tf.constant(0.0), ()) # Build network. - self._output_tensor = self._build_network(self._input_im_node, self._input_pose_node, self._input_drop_rate_node) - - # Add softmax function to output of network (this is optional because 1) we might be doing regression or 2) we are training and Tensorflow has an optimized cross-entropy loss with the softmax already built-in). + self._output_tensor = self._build_network( + self._input_im_node, self._input_pose_node, + self._input_drop_rate_node) + + # Add softmax function to output of network (this is optional + # because 1) we might be doing regression or 2) we are training and + # Tensorflow has an optimized cross-entropy loss with the softmax + # already built-in). if add_softmax: self.add_softmax_to_output() - # Add sigmoid function to output of network (for weighted cross-entropy loss). + # Add sigmoid function to output of network (for weighted + # cross-entropy loss). if add_sigmoid: self.add_sigmoid_to_output() # Create feed tensors for prediction. - self._input_im_arr = np.zeros((self._batch_size, self._im_height, self._im_width, self._num_channels)) + self._input_im_arr = np.zeros((self._batch_size, self._im_height, + self._im_width, self._num_channels)) self._input_pose_arr = np.zeros((self._batch_size, self._pose_dim)) def open_session(self): @@ -481,7 +561,8 @@ def open_session(self): with self._graph.as_default(): init = tf.global_variables_initializer() self.tf_config = tf.ConfigProto() - # Allow Tensorflow gpu growth so Tensorflow does not lock-up all GPU memory. + # Allow Tensorflow gpu growth so Tensorflow does not lock-up all + # GPU memory. self.tf_config.gpu_options.allow_growth = True self._sess = tf.Session(graph=self._graph, config=self.tf_config) self._sess.run(init) @@ -499,7 +580,7 @@ def close_session(self): def __del__(self): """Destructor that basically just makes sure the Tensorflow session - has been closed.""" + has been closed.""" if self._sess is not None: self.close_session() @@ -538,7 +619,7 @@ def input_im_node(self): @property def input_pose_node(self): return self._input_pose_node - + @property def input_drop_rate_node(self): return self._input_drop_rate_node @@ -554,7 +635,7 @@ def weights(self): @property def tf_graph(self): return self._graph - + @property def sess(self): return self._sess @@ -569,7 +650,11 @@ def max_angle(self): @property def stride(self): - return reduce(operator.mul, [layer["pool_stride"] for layer in self._architecture["im_stream"].values() if layer["type"] == "conv"]) + return reduce(operator.mul, [ + layer["pool_stride"] + for layer in self._architecture["im_stream"].values() + if layer["type"] == "conv" + ]) @property def filters(self): @@ -578,7 +663,7 @@ def filters(self): Returns ------- :obj:`numpy.ndarray` - filters (weights) from first convolution layer of the network + Filters (weights) from first convolution layer of the network. """ close_sess = False if self._sess is None: @@ -587,11 +672,13 @@ def filters(self): first_layer_name = list(self._architecture["im_stream"])[0] try: - filters = self._sess.run(self._weights.weights["{}_weights".format(first_layer_name)]) - except: + filters = self._sess.run( + self._weights.weights["{}_weights".format(first_layer_name)]) + except KeyError: # Legacy support. - filters = self._sess.run(self._weights.weights["{}W".format(first_layer_im_stream)]) - + filters = self._sess.run( + self._weights.weights["{}W".format(first_layer_name)]) + if close_sess: self.close_session() return filters @@ -602,114 +689,114 @@ def set_batch_size(self, batch_size): Parameters ---------- batch_size : int - the new batch size + The new batch size. """ self._batch_size = batch_size def set_im_mean(self, im_mean): - """Update image mean to be used for normalization during inference. - + """Update image mean to be used for normalization during inference. + Parameters ---------- im_mean : float - the new image mean + The new image mean. """ self._im_mean = im_mean - + def get_im_mean(self): """Get the current image mean used for normalization during inference. Returns ------- : float - the image mean + The image mean. """ return self.im_mean def set_im_std(self, im_std): """Update image standard deviation to be used for normalization during - inference. - + inference. + Parameters ---------- im_std : float - the new image standard deviation + The new image standard deviation. """ self._im_std = im_std def get_im_std(self): """Get the current image standard deviation to be used for - normalization during inference. + normalization during inference. Returns ------- : float - the image standard deviation + The image standard deviation. """ return self.im_std def set_pose_mean(self, pose_mean): """Update pose mean to be used for normalization during inference. - + Parameters ---------- pose_mean : :obj:`numpy.ndarray` - the new pose mean + The new pose mean. """ self._pose_mean = pose_mean def get_pose_mean(self): """Get the current pose mean to be used for normalization during - inference. + inference. Returns ------- :obj:`numpy.ndarray` - the pose mean + The pose mean. """ return self._pose_mean def set_pose_std(self, pose_std): """Update pose standard deviation to be used for normalization during - inference. - + inference. + Parameters ---------- pose_std : :obj:`numpy.ndarray` - the new pose standard deviation + The new pose standard deviation. """ self._pose_std = pose_std def get_pose_std(self): """Get the current pose standard deviation to be used for normalization - during inference. + during inference. Returns ------- :obj:`numpy.ndarray` - the pose standard deviation + The pose standard deviation. """ return self._pose_std def set_im_depth_sub_mean(self, im_depth_sub_mean): """Update mean of subtracted image and gripper depth to be used for - normalization during inference. - + normalization during inference. + Parameters ---------- im_depth_sub_mean : float - the new mean of subtracted image and gripper depth + The new mean of subtracted image and gripper depth. """ self._im_depth_sub_mean = im_depth_sub_mean def set_im_depth_sub_std(self, im_depth_sub_std): """Update standard deviation of subtracted image and gripper depth to - be used for normalization during inference. - + be used for normalization during inference. + Parameters ---------- im_depth_sub_std : float - the standard deviation of subtracted image and gripper depth + The standard deviation of subtracted image and gripper depth. """ self._im_depth_sub_std = im_depth_sub_std @@ -718,8 +805,12 @@ def add_softmax_to_output(self): with tf.name_scope("softmax"): if self._angular_bins > 0: self._logger.info("Building Pair-wise Softmax Layer...") - binwise_split_output = tf.split(self._output_tensor, self._angular_bins, axis=-1) - binwise_split_output_soft = [tf.nn.softmax(s) for s in binwise_split_output] + binwise_split_output = tf.split(self._output_tensor, + self._angular_bins, + axis=-1) + binwise_split_output_soft = [ + tf.nn.softmax(s) for s in binwise_split_output + ] self._output_tensor = tf.concat(binwise_split_output_soft, -1) else: self._logger.info("Building Softmax Layer...") @@ -732,12 +823,12 @@ def add_sigmoid_to_output(self): self._output_tensor = tf.nn.sigmoid(self._output_tensor) def update_batch_size(self, batch_size): - """Update the inference batch size. + """Update the inference batch size. Parameters ---------- batch_size : float - the new batch size + The new batch size. """ self._batch_size = batch_size @@ -747,13 +838,13 @@ def _predict(self, image_arr, pose_arr, verbose=False): Parameters ---------- image_arr : :obj:`numpy.ndarray` - input images + Input images. pose_arr : :obj:`numpy.ndarray` - input gripper poses + Input gripper poses. verbose : bool - whether or not to log progress to stdout, useful to turn off during - training - """ + Whether or not to log progress to `stdout`, useful to turn off + during training. + """ # Get prediction start time. start_time = time.time() @@ -772,40 +863,51 @@ def _predict(self, image_arr, pose_arr, verbose=False): # Predict in batches. with self._graph.as_default(): if self._sess is None: - raise RuntimeError("No TF Session open. Please call open_session() first.") + raise RuntimeError( + "No TF Session open. Please call open_session() first.") i = 0 batch_idx = 0 while i < num_images: if verbose: - self._logger.info("Predicting batch {} of {}...".format(batch_idx, num_batches)) + self._logger.info("Predicting batch {} of {}...".format( + batch_idx, num_batches)) batch_idx += 1 dim = min(self._batch_size, num_images - i) cur_ind = i end_ind = cur_ind + dim - + if self._input_depth_mode == InputDepthMode.POSE_STREAM: self._input_im_arr[:dim, ...] = ( - image_arr[cur_ind:end_ind, ...] - self._im_mean) / self._im_std + image_arr[cur_ind:end_ind, ...] - + self._im_mean) / self._im_std self._input_pose_arr[:dim, :] = ( - pose_arr[cur_ind:end_ind, :] - self._pose_mean) / self._pose_std + pose_arr[cur_ind:end_ind, :] - + self._pose_mean) / self._pose_std elif self._input_depth_mode == InputDepthMode.SUB: - self._input_im_arr[:dim, ...] = image_arr[cur_ind:end_ind, ...] - self._input_pose_arr[:dim, :] = pose_arr[cur_ind:end_ind, :] + self._input_im_arr[:dim, ...] = image_arr[cur_ind:end_ind, + ...] + self._input_pose_arr[:dim, :] = pose_arr[cur_ind: + end_ind, :] elif self._input_depth_mode == InputDepthMode.IM_ONLY: self._input_im_arr[:dim, ...] = ( - image_arr[cur_ind:end_ind, ...] - self._im_mean) / self._im_std + image_arr[cur_ind:end_ind, ...] - + self._im_mean) / self._im_std - gqcnn_output = self._sess.run(self._output_tensor, - feed_dict={self._input_im_node: self._input_im_arr, - self._input_pose_node: self._input_pose_arr}) + gqcnn_output = self._sess.run( + self._output_tensor, + feed_dict={ + self._input_im_node: self._input_im_arr, + self._input_pose_node: self._input_pose_arr + }) # Allocate output tensor. if output_arr is None: - output_arr = np.zeros([num_images] + list(gqcnn_output.shape[1:])) + output_arr = np.zeros([num_images] + + list(gqcnn_output.shape[1:])) output_arr[cur_ind:end_ind, :] = gqcnn_output[:dim, :] i = end_ind - + # Get total prediction time. pred_time = time.time() - start_time if verbose: @@ -815,33 +917,37 @@ def _predict(self, image_arr, pose_arr, verbose=False): def predict(self, image_arr, pose_arr, verbose=False): """Predict the probability of grasp success given a depth image and - gripper pose + gripper pose. Parameters ---------- image_arr : :obj:`numpy ndarray` - 4D tensor of depth images + 4D tensor of depth images. pose_arr : :obj:`numpy ndarray` - tensor of gripper poses + Tensor of gripper poses. verbose : bool - whether or not to log progress to stdout, useful to turn off during - training + Whether or not to log progress to stdout, useful to turn off during + training. """ return self._predict(image_arr, pose_arr, verbose=verbose) - - def featurize(self, image_arr, pose_arr=None, feature_layer="conv1_1", verbose=False): + + def featurize(self, + image_arr, + pose_arr=None, + feature_layer="conv1_1", + verbose=False): """Featurize a set of inputs. - + Parameters ---------- - image_arr : :obj:`numpy ndarray` - 4D tensor of depth images + image_arr : :obj:`numpy ndarray` + 4D tensor of depth images. pose_arr : :obj:`numpy ndarray` - optional tensor of gripper poses + Optional tensor of gripper poses. feature_layer : str - the network layer to featurize + The network layer to featurize. verbose : bool - whether or not to log progress to stdout + Whether or not to log progress to `stdout`. """ # Get featurization start time. start_time = time.time() @@ -850,130 +956,176 @@ def featurize(self, image_arr, pose_arr=None, feature_layer="conv1_1", verbose=F self._logger.info("Featurizing...") if feature_layer not in self._feature_tensors: - raise ValueError("Feature layer: {} not recognized.".format(feature_layer)) - + raise ValueError( + "Feature layer: {} not recognized.".format(feature_layer)) + # Setup for featurization. num_images = image_arr.shape[0] if pose_arr is not None: num_poses = pose_arr.shape[0] if num_images != num_poses: - raise ValueError("Must provide same number of images as poses!") + raise ValueError( + "Must provide same number of images as poses!") output_arr = None # Featurize in batches. with self._graph.as_default(): if self._sess is None: - raise RuntimeError("No TF Session open. Please call open_session() first.") + raise RuntimeError( + "No TF Session open. Please call open_session() first.") i = 0 while i < num_images: if verbose: - self._logger.info("Featurizing {} of {}...".format(i, num_images)) + self._logger.info("Featurizing {} of {}...".format( + i, num_images)) dim = min(self._batch_size, num_images - i) cur_ind = i end_ind = cur_ind + dim self._input_im_arr[:dim, :, :, :] = ( - image_arr[cur_ind:end_ind, :, :, :] - self._im_mean) / self._im_std + image_arr[cur_ind:end_ind, :, :, :] - + self._im_mean) / self._im_std if pose_arr is not None: self._input_pose_arr[:dim, :] = ( - pose_arr[cur_ind:end_ind, :] - self._pose_mean) / self._pose_std + pose_arr[cur_ind:end_ind, :] - + self._pose_mean) / self._pose_std if pose_arr is not None: - gqcnn_output = self._sess.run(self._feature_tensors[feature_layer], - feed_dict={self._input_im_node: self._input_im_arr, - self._input_pose_node: self._input_pose_arr}) + gqcnn_output = self._sess.run( + self._feature_tensors[feature_layer], + feed_dict={ + self._input_im_node: self._input_im_arr, + self._input_pose_node: self._input_pose_arr + }) else: - gqcnn_output = self._sess.run(self._feature_tensors[feature_layer], - feed_dict={self._input_im_node: self._input_im_arr}) + gqcnn_output = self._sess.run( + self._feature_tensors[feature_layer], + feed_dict={self._input_im_node: self._input_im_arr}) if output_arr is None: - output_arr = np.zeros([num_images] + list(gqcnn_output.shape[1:])) + output_arr = np.zeros([num_images] + + list(gqcnn_output.shape[1:])) output_arr[cur_ind:end_ind, :] = gqcnn_output[:dim, :] i = end_ind if verbose: - self._logger.info("Featurization took {} seconds".format(time.time() - start_time)) + self._logger.info( + "Featurization took {} seconds".format(time.time() - + start_time)) # Truncate extraneous values off of end of `output_arr`. - output_arr = output_arr[:num_images] # TODO(vsatish): This isn"t needed, right? + # TODO(vsatish): This isn't needed, right? + output_arr = output_arr[:num_images] return output_arr - + def _leaky_relu(self, x, alpha=.1): return tf.maximum(alpha * x, x) - - def _build_conv_layer(self, input_node, input_height, input_width, input_channels, filter_h, filter_w, num_filt, pool_stride_h, pool_stride_w, pool_size, name, norm=False, pad="SAME"): - self._logger.info("Building convolutional layer: {}...".format(name)) + + def _build_conv_layer(self, + input_node, + input_height, + input_width, + input_channels, + filter_h, + filter_w, + num_filt, + pool_stride_h, + pool_stride_w, + pool_size, + name, + norm=False, + pad="SAME"): + self._logger.info("Building convolutional layer: {}...".format(name)) with tf.name_scope(name): # Initialize weights. if "{}_weights".format(name) in self._weights.weights: convW = self._weights.weights["{}_weights".format(name)] - convb = self._weights.weights["{}_bias".format(name)] - elif "{}W".format(name) in self._weights.weights: # Legacy support. - self._logger.info("Using old format for layer {}.".format(name)) + convb = self._weights.weights["{}_bias".format(name)] + elif "{}W".format( + name) in self._weights.weights: # Legacy support. + self._logger.info( + "Using old format for layer {}.".format(name)) convW = self._weights.weights["{}W".format(name)] - convb = self._weights.weights["{}b".format(name)] + convb = self._weights.weights["{}b".format(name)] else: self._logger.info("Reinitializing layer {}.".format(name)) convW_shape = [filter_h, filter_w, input_channels, num_filt] fan_in = filter_h * filter_w * input_channels std = np.sqrt(2 / fan_in) - convW = tf.Variable(tf.truncated_normal(convW_shape, stddev=std), name="{}_weights".format(name)) - convb = tf.Variable(tf.truncated_normal([num_filt], stddev=std), name="{}_bias".format(name)) + convW = tf.Variable(tf.truncated_normal(convW_shape, + stddev=std), + name="{}_weights".format(name)) + convb = tf.Variable(tf.truncated_normal([num_filt], + stddev=std), + name="{}_bias".format(name)) self._weights.weights["{}_weights".format(name)] = convW self._weights.weights["{}_bias".format(name)] = convb - + if pad == "SAME": out_height = input_height // pool_stride_h out_width = input_width // pool_stride_w else: - out_height = math.ceil((input_height - filter_h + 1) / pool_stride_h) - out_width = math.ceil((input_width - filter_w + 1) / pool_stride_w) + out_height = math.ceil( + (input_height - filter_h + 1) / pool_stride_h) + out_width = math.ceil( + (input_width - filter_w + 1) / pool_stride_w) out_channels = num_filt # Build layer. - convh = tf.nn.conv2d(input_node, convW, strides=[ - 1, 1, 1, 1], padding=pad) + convb + convh = tf.nn.conv2d( + input_node, convW, strides=[1, 1, 1, 1], padding=pad) + convb convh = self._leaky_relu(convh, alpha=self._relu_coeff) - + if norm: - convh = tf.nn.local_response_normalization(convh, - depth_radius=self._normalization_radius, - alpha=self._normalization_alpha, - beta=self._normalization_beta, - bias=self._normalization_bias) + convh = tf.nn.local_response_normalization( + convh, + depth_radius=self._normalization_radius, + alpha=self._normalization_alpha, + beta=self._normalization_beta, + bias=self._normalization_bias) pool = tf.nn.max_pool(convh, - ksize=[1, pool_size, pool_size, 1], - strides=[1, pool_stride_h, - pool_stride_w, 1], - padding="SAME") - + ksize=[1, pool_size, pool_size, 1], + strides=[1, pool_stride_h, pool_stride_w, 1], + padding="SAME") + # Add output to feature dict. self._feature_tensors[name] = pool return pool, out_height, out_width, out_channels - def _build_fc_layer(self, input_node, fan_in, out_size, name, input_is_multi, drop_rate, final_fc_layer=False): + def _build_fc_layer(self, + input_node, + fan_in, + out_size, + name, + input_is_multi, + drop_rate, + final_fc_layer=False): self._logger.info("Building fully connected layer: {}...".format(name)) - + # Initialize weights. if "{}_weights".format(name) in self._weights.weights: fcW = self._weights.weights["{}_weights".format(name)] - fcb = self._weights.weights["{}_bias".format(name)] - elif "{}W".format(name) in self._weights.weights: # Legacy support. + fcb = self._weights.weights["{}_bias".format(name)] + elif "{}W".format(name) in self._weights.weights: # Legacy support. self._logger.info("Using old format for layer {}.".format(name)) fcW = self._weights.weights["{}W".format(name)] - fcb = self._weights.weights["{}b".format(name)] + fcb = self._weights.weights["{}b".format(name)] else: self._logger.info("Reinitializing layer {}.".format(name)) std = np.sqrt(2 / fan_in) - fcW = tf.Variable(tf.truncated_normal([fan_in, out_size], stddev=std), name="{}_weights".format(name)) + fcW = tf.Variable(tf.truncated_normal([fan_in, out_size], + stddev=std), + name="{}_weights".format(name)) if final_fc_layer: - fcb = tf.Variable(tf.constant(0.0, shape=[out_size]), name="{}_bias".format(name)) + fcb = tf.Variable(tf.constant(0.0, shape=[out_size]), + name="{}_bias".format(name)) else: - fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), name="{}_bias".format(name)) + fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), + name="{}_bias".format(name)) self._weights.weights["{}_weights".format(name)] = fcW self._weights.weights["{}_bias".format(name)] = fcb @@ -985,7 +1137,8 @@ def _build_fc_layer(self, input_node, fan_in, out_size, name, input_is_multi, dr if final_fc_layer: fc = tf.matmul(input_node, fcW) + fcb else: - fc = self._leaky_relu(tf.matmul(input_node, fcW) + fcb, alpha=self._relu_coeff) + fc = self._leaky_relu(tf.matmul(input_node, fcW) + fcb, + alpha=self._relu_coeff) fc = tf.nn.dropout(fc, 1 - drop_rate) @@ -994,57 +1147,66 @@ def _build_fc_layer(self, input_node, fan_in, out_size, name, input_is_multi, dr return fc, out_size - # TODO(vsatish): This really doesn"t need to it"s own layer type...it does the same thing as `_build_fc_layer`. + # TODO(vsatish): This really doesn"t need to it"s own layer type...it does + # the same thing as `_build_fc_layer`. def _build_pc_layer(self, input_node, fan_in, out_size, name): - self._logger.info("Building Fully Connected Pose Layer: {}...".format(name)) - + self._logger.info( + "Building Fully Connected Pose Layer: {}...".format(name)) + # Initialize weights. if "{}_weights".format(name) in self._weights.weights: pcW = self._weights.weights["{}_weights".format(name)] - pcb = self._weights.weights["{}_bias".format(name)] - elif "{}W".format(name) in self._weights.weights: # Legacy support. + pcb = self._weights.weights["{}_bias".format(name)] + elif "{}W".format(name) in self._weights.weights: # Legacy support. self._logger.info("Using old format for layer {}".format(name)) pcW = self._weights.weights["{}W".format(name)] - pcb = self._weights.weights["{}b".format(name)] + pcb = self._weights.weights["{}b".format(name)] else: self._logger.info("Reinitializing layer {}".format(name)) std = np.sqrt(2 / fan_in) pcW = tf.Variable(tf.truncated_normal([fan_in, out_size], - stddev=std), name="{}_weights".format(name)) - pcb = tf.Variable(tf.truncated_normal([out_size], - stddev=std), name="{}_bias".format(name)) + stddev=std), + name="{}_weights".format(name)) + pcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), + name="{}_bias".format(name)) self._weights.weights["{}_weights".format(name)] = pcW self._weights.weights["{}_bias".format(name)] = pcb # Build layer. - pc = self._leaky_relu(tf.matmul(input_node, pcW) + - pcb, alpha=self._relu_coeff) + pc = self._leaky_relu(tf.matmul(input_node, pcW) + pcb, + alpha=self._relu_coeff) # Add output to feature dict. self._feature_tensors[name] = pc return pc, out_size - def _build_fc_merge(self, input_fc_node_1, input_fc_node_2, fan_in_1, fan_in_2, out_size, drop_rate, name): + def _build_fc_merge(self, input_fc_node_1, input_fc_node_2, fan_in_1, + fan_in_2, out_size, drop_rate, name): self._logger.info("Building Merge Layer: {}...".format(name)) - + # Initialize weights. if "{}_input_1_weights".format(name) in self._weights.weights: input1W = self._weights.weights["{}_input_1_weights".format(name)] input2W = self._weights.weights["{}_input_2_weights".format(name)] - fcb = self._weights.weights["{}_bias".format(name)] - elif "{}W_im".format(name) in self._weights.weights: # Legacy support. + fcb = self._weights.weights["{}_bias".format(name)] + elif "{}W_im".format(name) in self._weights.weights: # Legacy support. self._logger.info("Using old format for layer {}.".format(name)) input1W = self._weights.weights["{}W_im".format(name)] input2W = self._weights.weights["{}W_pose".format(name)] - fcb = self._weights.weights["{}b".format(name)] + fcb = self._weights.weights["{}b".format(name)] else: self._logger.info("Reinitializing layer {}.".format(name)) std = np.sqrt(2 / (fan_in_1 + fan_in_2)) - input1W = tf.Variable(tf.truncated_normal([fan_in_1, out_size], stddev=std), name="{}_input_1_weights".format(name)) - input2W = tf.Variable(tf.truncated_normal([fan_in_2, out_size], stddev=std), name="{}_input_2_weights".format(name)) - fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), name="{}_bias".format(name)) + input1W = tf.Variable(tf.truncated_normal([fan_in_1, out_size], + stddev=std), + name="{}_input_1_weights".format(name)) + input2W = tf.Variable(tf.truncated_normal([fan_in_2, out_size], + stddev=std), + name="{}_input_2_weights".format(name)) + fcb = tf.Variable(tf.truncated_normal([out_size], stddev=std), + name="{}_bias".format(name)) self._weights.weights["{}_input_1_weights".format(name)] = input1W self._weights.weights["{}_input_2_weights".format(name)] = input2W @@ -1052,8 +1214,8 @@ def _build_fc_merge(self, input_fc_node_1, input_fc_node_2, fan_in_1, fan_in_2, # Build layer. fc = self._leaky_relu(tf.matmul(input_fc_node_1, input1W) + - tf.matmul(input_fc_node_2, input2W) + - fcb, alpha=self._relu_coeff) + tf.matmul(input_fc_node_2, input2W) + fcb, + alpha=self._relu_coeff) fc = tf.nn.dropout(fc, 1 - drop_rate) # Add output to feature dict. @@ -1061,26 +1223,53 @@ def _build_fc_merge(self, input_fc_node_1, input_fc_node_2, fan_in_1, fan_in_2, return fc, out_size - def _build_im_stream(self, input_node, input_pose_node, input_height, input_width, input_channels, drop_rate, layers, only_stream=False): + def _build_im_stream(self, + input_node, + input_pose_node, + input_height, + input_width, + input_channels, + drop_rate, + layers, + only_stream=False): self._logger.info("Building Image Stream...") if self._input_depth_mode == InputDepthMode.SUB: sub_mean = tf.constant(self._im_depth_sub_mean, dtype=tf.float32) sub_std = tf.constant(self._im_depth_sub_std, dtype=tf.float32) - sub_im = tf.subtract(input_node, tf.tile(tf.reshape(input_pose_node, tf.constant((-1, 1, 1, 1))), tf.constant((1, input_height, input_width, 1)))) + sub_im = tf.subtract( + input_node, + tf.tile( + tf.reshape(input_pose_node, tf.constant((-1, 1, 1, 1))), + tf.constant((1, input_height, input_width, 1)))) norm_sub_im = tf.div(tf.subtract(sub_im, sub_mean), sub_std) input_node = norm_sub_im output_node = input_node - prev_layer = "start" # Dummy placeholder. + prev_layer = "start" # Dummy placeholder. last_index = len(layers) - 1 - for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + for layer_index, (layer_name, + layer_config) in enumerate(layers.items()): layer_type = layer_config["type"] if layer_type == "conv": if prev_layer == "fc": raise ValueError("Cannot have conv layer after fc layer!") - output_node, input_height, input_width, input_channels = self._build_conv_layer(output_node, input_height, input_width, input_channels, layer_config["filt_dim"], layer_config["filt_dim"], layer_config["num_filt"], layer_config["pool_stride"], layer_config["pool_stride"], layer_config["pool_size"], layer_name, norm=layer_config["norm"], pad=layer_config["pad"]) - prev_layer = layer_type + output_node, input_height, input_width, input_channels = \ + self._build_conv_layer( + output_node, + input_height, + input_width, + input_channels, + layer_config["filt_dim"], + layer_config["filt_dim"], + layer_config["num_filt"], + layer_config["pool_stride"], + layer_config["pool_stride"], + layer_config["pool_size"], + layer_name, + norm=layer_config["norm"], + pad=layer_config["pad"]) + prev_layer = layer_type elif layer_type == "fc": if layer_config["out_size"] == 0: continue @@ -1089,102 +1278,155 @@ def _build_im_stream(self, input_node, input_pose_node, input_height, input_widt prev_layer_is_conv = True fan_in = input_height * input_width * input_channels if layer_index == last_index and only_stream: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, prev_layer_is_conv, drop_rate, final_fc_layer=True) + output_node, fan_in = self._build_fc_layer( + output_node, + fan_in, + layer_config["out_size"], + layer_name, + prev_layer_is_conv, + drop_rate, + final_fc_layer=True) else: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, prev_layer_is_conv, drop_rate) + output_node, fan_in = self._build_fc_layer( + output_node, fan_in, layer_config["out_size"], + layer_name, prev_layer_is_conv, drop_rate) prev_layer = layer_type elif layer_type == "pc": - raise ValueError("Cannot have pose connected layer in image stream!") + raise ValueError( + "Cannot have pose connected layer in image stream!") elif layer_type == "fc_merge": raise ValueError("Cannot have merge layer in image stream!") else: - raise ValueError("Unsupported layer type: {}".format(layer_type)) + raise ValueError( + "Unsupported layer type: {}".format(layer_type)) return output_node, fan_in def _build_pose_stream(self, input_node, fan_in, layers): self._logger.info("Building Pose Stream...") output_node = input_node - prev_layer = "start" # Dummy placeholder. for layer_name, layer_config in layers.items(): layer_type = layer_config["type"] if layer_type == "conv": - raise ValueError("Cannot have conv layer in pose stream") + raise ValueError("Cannot have conv layer in pose stream") elif layer_type == "fc": - raise ValueError("Cannot have fully connected layer in pose stream") + raise ValueError( + "Cannot have fully connected layer in pose stream") elif layer_type == "pc": if layer_config["out_size"] == 0: continue - output_node, fan_in = self._build_pc_layer(output_node, fan_in, layer_config["out_size"], layer_name) - prev_layer = layer_type + output_node, fan_in = self._build_pc_layer( + output_node, fan_in, layer_config["out_size"], layer_name) elif layer_type == "fc_merge": raise ValueError("Cannot have merge layer in pose stream") else: - raise ValueError("Unsupported layer type: {}".format(layer_type)) + raise ValueError( + "Unsupported layer type: {}".format(layer_type)) return output_node, fan_in - def _build_merge_stream(self, input_stream_1, input_stream_2, fan_in_1, fan_in_2, drop_rate, layers): + def _build_merge_stream(self, input_stream_1, input_stream_2, fan_in_1, + fan_in_2, drop_rate, layers): self._logger.info("Building Merge Stream...") - + # First check if first layer is a merge layer. # TODO(vsatish): Can't we just get the first value because it's # ordered? if layers[list(layers)[0]]["type"] != "fc_merge": - raise ValueError("First layer in merge stream must be a fc_merge layer!") - - prev_layer = "start" + raise ValueError( + "First layer in merge stream must be a fc_merge layer!") + last_index = len(layers) - 1 fan_in = -1 - for layer_index, (layer_name, layer_config) in enumerate(layers.items()): + output_node = None # Will be overridden. + for layer_index, (layer_name, + layer_config) in enumerate(layers.items()): layer_type = layer_config["type"] if layer_type == "conv": - raise ValueError("Cannot have conv layer in merge stream!") + raise ValueError("Cannot have conv layer in merge stream!") elif layer_type == "fc": if layer_config["out_size"] == 0: continue if layer_index == last_index: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, False, drop_rate, final_fc_layer=True) + output_node, fan_in = self._build_fc_layer( + output_node, + fan_in, + layer_config["out_size"], + layer_name, + False, + drop_rate, + final_fc_layer=True) else: - output_node, fan_in = self._build_fc_layer(output_node, fan_in, layer_config["out_size"], layer_name, False, drop_rate) - prev_layer = layer_type - elif layer_type == "pc": - raise ValueError("Cannot have pose connected layer in merge stream!") + output_node, fan_in = self._build_fc_layer( + output_node, fan_in, layer_config["out_size"], + layer_name, False, drop_rate) + elif layer_type == "pc": + raise ValueError( + "Cannot have pose connected layer in merge stream!") elif layer_type == "fc_merge": if layer_config["out_size"] == 0: continue - output_node, fan_in = self._build_fc_merge(input_stream_1, input_stream_2, fan_in_1, fan_in_2, layer_config["out_size"], drop_rate, layer_name) - prev_layer = layer_type + output_node, fan_in = self._build_fc_merge( + input_stream_1, input_stream_2, fan_in_1, fan_in_2, + layer_config["out_size"], drop_rate, layer_name) else: - raise ValueError("Unsupported layer type: {}".format(layer_type)) + raise ValueError( + "Unsupported layer type: {}".format(layer_type)) return output_node, fan_in - def _build_network(self, input_im_node, input_pose_node, input_drop_rate_node): + def _build_network(self, input_im_node, input_pose_node, + input_drop_rate_node): """Build GQ-CNN. Parameters ---------- input_im_node :obj:`tf.placeholder` - image placeholder + Image placeholder. input_pose_node :obj:`tf.placeholder` - gripper pose placeholder + Gripper pose placeholder. input_drop_rate_node :obj:`tf.placeholder` - drop rate placeholder + Drop rate placeholder. Returns ------- :obj:`tf.Tensor` - tensor output of network + Tensor output of network. """ self._logger.info("Building Network...") if self._input_depth_mode == InputDepthMode.POSE_STREAM: - assert "pose_stream" in self._architecture and "merge_stream" in self._architecture, "When using input depth mode \"pose_stream\", both pose stream and merge stream must be present!" + missing_stream_msg = ("When using input depth mode" + " \"pose_stream\", both pose stream and" + " merge stream must be present!") + assert "pose_stream" in self._architecture and \ + "merge_stream" in self._architecture, missing_stream_msg with tf.name_scope("im_stream"): - output_im_stream, fan_out_im = self._build_im_stream(input_im_node, input_pose_node, self._im_height, self._im_width, self._num_channels, input_drop_rate_node, self._architecture["im_stream"]) + output_im_stream, fan_out_im = self._build_im_stream( + input_im_node, input_pose_node, self._im_height, + self._im_width, self._num_channels, input_drop_rate_node, + self._architecture["im_stream"]) with tf.name_scope("pose_stream"): - output_pose_stream, fan_out_pose = self._build_pose_stream(input_pose_node, self._pose_dim, self._architecture["pose_stream"]) + output_pose_stream, fan_out_pose = self._build_pose_stream( + input_pose_node, self._pose_dim, + self._architecture["pose_stream"]) with tf.name_scope("merge_stream"): - return self._build_merge_stream(output_im_stream, output_pose_stream, fan_out_im, fan_out_pose, input_drop_rate_node, self._architecture["merge_stream"])[0] - elif self._input_depth_mode == InputDepthMode.SUB or self._input_depth_mode == InputDepthMode.IM_ONLY: - assert not ("pose_stream" in self._architecture or "merge_stream" in self._architecture), "When using input depth mode \"{}\", only im stream is allowed!".format(self._input_depth_mode) + return self._build_merge_stream( + output_im_stream, output_pose_stream, fan_out_im, + fan_out_pose, input_drop_rate_node, + self._architecture["merge_stream"])[0] + elif self._input_depth_mode == InputDepthMode.SUB or \ + self._input_depth_mode == InputDepthMode.IM_ONLY: + extraneous_stream_msg = ("When using input depth mode \"{}\", only" + " im stream is allowed!") + assert not ( + "pose_stream" in self._architecture + or "merge_stream" in self._architecture + ), extraneous_stream_msg.format( + self._input_depth_mode) with tf.name_scope("im_stream"): - return self._build_im_stream(input_im_node, input_pose_node, self._im_height, self._im_width, self._num_channels, input_drop_rate_node, self._architecture["im_stream"], only_stream=True)[0] + return self._build_im_stream(input_im_node, + input_pose_node, + self._im_height, + self._im_width, + self._num_channels, + input_drop_rate_node, + self._architecture["im_stream"], + only_stream=True)[0] diff --git a/gqcnn/search/enums.py b/gqcnn/search/enums.py index 17b516c7..184e5ab5 100644 --- a/gqcnn/search/enums.py +++ b/gqcnn/search/enums.py @@ -23,17 +23,27 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Enums for hyper-parameter search. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division from __future__ import print_function + class TrialConstants(object): - TRIAL_CPU_LOAD = 300 # Decrease this to get more aggressize CPU utilization. - TRIAL_GPU_LOAD = 33 # Decrease this to get more aggressize GPU utilization. - TRIAL_GPU_MEM = 2000 # This really depends on model size(TRIAL_GPU_LOAD does too,but it's not a hard limit per se). Ideally we would initialize models one by one and monitor the space left, but because model initialization comes after some metric calculation, we set this to be some upper bound based on the largest model and do batch initalizations from there. + TRIAL_CPU_LOAD = 300 # Decrease to get more aggressize CPU utilization. + TRIAL_GPU_LOAD = 33 # Decrease to get more aggressize GPU utilization. + # This really depends on model size (`TRIAL_GPU_LOAD` does too, but it's + # not a hard limit per se). Ideally we would initialize models one-by-one + # and monitor the space left, but because model initialization comes after + # some metric calculation, we set this to be some upper bound based on the + # largest model and do batch initalizations from there. + TRIAL_GPU_MEM = 2000 + class SearchConstants(object): - SEARCH_THREAD_SLEEP = 2 + SEARCH_THREAD_SLEEP = 2 MIN_TIME_BETWEEN_SCHEDULE_ATTEMPTS = 20 diff --git a/gqcnn/search/resource_manager.py b/gqcnn/search/resource_manager.py index 5fa3432c..2de1b8f4 100644 --- a/gqcnn/search/resource_manager.py +++ b/gqcnn/search/resource_manager.py @@ -24,16 +24,17 @@ Intelligent resource manager for hyper-parameter search. Queries resources available and appropriately distributes resources over possible trials to run. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division from __future__ import print_function import math -import os import random -import sys import time import GPUtil @@ -43,12 +44,22 @@ from autolab_core import Logger CPU_LOAD_SAMPLE_INTERVAL = 4.0 -CPU_LOAD_OFFSET = 50 # This is a hack because it seems that psutil is returning a lower load than htop, which could be because htop takes into account queued tasks. +# This is a hack because it seems that psutil is returning a lower load than +# htop, which could be because htop takes into account queued tasks. +CPU_LOAD_OFFSET = 50 GPU_STAT_NUM_SAMPLES = 4 GPU_STAT_SAMPLE_INTERVAL = 1.0 + class ResourceManager(object): - def __init__(self, trial_cpu_load, trial_gpu_load, trial_gpu_mem, monitor_cpu=True, monitor_gpu=True, cpu_cores=[], gpu_devices=[]): + def __init__(self, + trial_cpu_load, + trial_gpu_load, + trial_gpu_mem, + monitor_cpu=True, + monitor_gpu=True, + cpu_cores=[], + gpu_devices=[]): self._monitor_cpu = monitor_cpu self._monitor_gpu = monitor_gpu @@ -56,9 +67,11 @@ def __init__(self, trial_cpu_load, trial_gpu_load, trial_gpu_mem, monitor_cpu=Tr self._logger = Logger.get_logger(self.__class__.__name__) if not monitor_cpu: - self._logger.warning("Not monitoring cpu resources is not advised.") + self._logger.warning( + "Not monitoring cpu resources is not advised.") if not monitor_gpu: - self._logger.warning("Not monitoring gpu resources is not advised.") + self._logger.warning( + "Not monitoring gpu resources is not advised.") self._trial_cpu_load = trial_cpu_load self._trial_gpu_load = trial_gpu_load @@ -66,13 +79,17 @@ def __init__(self, trial_cpu_load, trial_gpu_load, trial_gpu_mem, monitor_cpu=Tr self._cpu_cores = cpu_cores if len(self._cpu_cores) == 0: - self._logger.warning("No CPU cores specified-proceeding to use all available cores.") + self._logger.warning( + "No CPU cores specified-proceeding to use all available cores." + ) self._cpu_cores = range(psutil.cpu_count()) self._cpu_count = len(self._cpu_cores) self._gpu_devices = gpu_devices if len(self._gpu_devices) == 0: - self._logger.warning("No GPU devices specified-proceeding to use all available devices.") + no_gpus_specified_msg = ("No GPU devices specified-proceeding to" + " use all available devices.") + self._logger.warning(no_gpus_specified_msg) self._gpu_devices = range(len(GPUtil.getGPUs())) @property @@ -81,7 +98,8 @@ def cpu_cores(self): def _get_cpu_load(self): self._logger.info("Sampling cpu load...") - cpu_core_loads = psutil.cpu_percent(interval=CPU_LOAD_SAMPLE_INTERVAL, percpu=True) + cpu_core_loads = psutil.cpu_percent(interval=CPU_LOAD_SAMPLE_INTERVAL, + percpu=True) total_load = 0 for core in self._cpu_cores: total_load += cpu_core_loads[core] @@ -96,50 +114,78 @@ def _get_gpu_stats(self): num_gpus = len(gpu_samples[0]) sample_loads = np.zeros((num_gpus, GPU_STAT_NUM_SAMPLES)) sample_mems = np.zeros((num_gpus, GPU_STAT_NUM_SAMPLES)) - total_mems = np.zeros((num_gpus,)) + total_mems = np.zeros((num_gpus, )) for i in range(GPU_STAT_NUM_SAMPLES): for gpu in gpu_samples[i]: if gpu.id in self._gpu_devices: sample_loads[gpu.id, i] = gpu.load * 100 sample_mems[gpu.id, i] = gpu.memoryUsed - else: # Trick the manager into thinking the GPU is fully utilized so it will never be chosen. + else: + # Trick the manager into thinking the GPU is fully utilized + # so it will never be chosen. sample_loads[gpu.id, i] = 100 - sample_mems[gpu.id, i] = gpu.memoryTotal + sample_mems[gpu.id, i] = gpu.memoryTotal total_mems[gpu.id] = gpu.memoryTotal - return total_mems.tolist(), np.mean(sample_loads, axis=1).tolist(), np.mean(sample_mems, axis=1).tolist() + return total_mems.tolist(), np.mean( + sample_loads, axis=1).tolist(), np.mean(sample_mems, + axis=1).tolist() def _build_gpu_list(self, max_possible_trials_per_device): gpus_avail = [] for device_id, max_trials in enumerate(max_possible_trials_per_device): for _ in range(max_trials): gpus_avail.append(str(device_id)) - random.shuffle(gpus_avail) # This is because we might truncate this list later because of a more severe resource bottleneck, in which case we want to evenly distribute the load. + # This is because we might truncate this list later because of a more + # severe resource bottleneck, in which case we want to evenly + # distribute the load. + random.shuffle(gpus_avail) return gpus_avail def num_trials_to_schedule(self, num_pending_trials): num_trials_to_schedule = num_pending_trials - if self._monitor_cpu: # Check cpu bandwith. + if self._monitor_cpu: # Check cpu bandwith. cpu_load = min(self._get_cpu_load(), self._cpu_count * 100) - max_possible_trials_cpu = int((self._cpu_count * 100 - cpu_load) // self._trial_cpu_load) - self._logger.info("CPU load: {}%, Max possible trials: {}".format(cpu_load, max_possible_trials_cpu)) - num_trials_to_schedule = min(num_trials_to_schedule, max_possible_trials_cpu) - - if self._monitor_gpu: # Check gpu bandwith. + max_possible_trials_cpu = int( + (self._cpu_count * 100 - cpu_load) // self._trial_cpu_load) + self._logger.info("CPU load: {}%, Max possible trials: {}".format( + cpu_load, max_possible_trials_cpu)) + num_trials_to_schedule = min(num_trials_to_schedule, + max_possible_trials_cpu) + + if self._monitor_gpu: # Check gpu bandwith. total_gpu_mems, gpu_loads, gpu_mems = self._get_gpu_stats() - max_possible_trials_gpu_load_per_device = [int((100 - gpu_load) // self._trial_gpu_load) for gpu_load in gpu_loads] - max_possible_trials_gpu_mem_per_device = [int((total_gpu_mem - gpu_mem) // self._trial_gpu_mem) for total_gpu_mem, gpu_mem in zip(total_gpu_mems, gpu_mems)] - max_possible_trials_gpu_per_device = map(lambda x: min(x[0], x[1]), zip(max_possible_trials_gpu_load_per_device, max_possible_trials_gpu_mem_per_device)) - self._logger.info("GPU loads: {}, GPU mems: {}, Max possible trials: {}".format("% ".join([str(gpu_load) for gpu_load in gpu_loads]) + "%", "MiB ".join([str(gpu_mem) for gpu_mem in gpu_mems]) + "MiB", sum(max_possible_trials_gpu_per_device))) - num_trials_to_schedule = min(num_trials_to_schedule, sum(max_possible_trials_gpu_per_device)) - + max_possible_trials_gpu_load_per_device = [ + int((100 - gpu_load) // self._trial_gpu_load) + for gpu_load in gpu_loads + ] + max_possible_trials_gpu_mem_per_device = [ + int((total_gpu_mem - gpu_mem) // self._trial_gpu_mem) + for total_gpu_mem, gpu_mem in zip(total_gpu_mems, gpu_mems) + ] + max_possible_trials_gpu_per_device = map( + lambda x: min(x[0], x[1]), + zip(max_possible_trials_gpu_load_per_device, + max_possible_trials_gpu_mem_per_device)) + self._logger.info( + "GPU loads: {}, GPU mems: {}, Max possible trials: {}".format( + "% ".join([str(gpu_load) for gpu_load in gpu_loads]) + "%", + "MiB ".join([str(gpu_mem) + for gpu_mem in gpu_mems]) + "MiB", + sum(max_possible_trials_gpu_per_device))) + num_trials_to_schedule = min( + num_trials_to_schedule, + sum(max_possible_trials_gpu_per_device)) + # Build the device list for scheduling trials on specific gpus. - gpus_avail = self._build_gpu_list(max_possible_trials_gpu_per_device) + gpus_avail = self._build_gpu_list( + max_possible_trials_gpu_per_device) else: # Just distribute load among gpus. num_gpus = self._get_gpu_count() trials_per_gpu = int(math.ceil(num_trials_to_schedule / num_gpus)) gpus_avail = self._build_gpu_list([trials_per_gpu] * num_gpus) - gpus_avail = gpus_avail[:num_trials_to_schedule] - self._logger.info("Max possible trials overall: {}".format(num_trials_to_schedule)) + gpus_avail = gpus_avail[:num_trials_to_schedule] + self._logger.info( + "Max possible trials overall: {}".format(num_trials_to_schedule)) return num_trials_to_schedule, gpus_avail diff --git a/gqcnn/search/search.py b/gqcnn/search/search.py index f53a9242..85b252a8 100644 --- a/gqcnn/search/search.py +++ b/gqcnn/search/search.py @@ -25,17 +25,16 @@ Perform hyper-parameter search over a set of GQ-CNN model/training parameters. Actively monitor system resources and appropriately schedule trials. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division from __future__ import print_function -import math -import multiprocessing -import operator import os -import sys import time from .resource_manager import ResourceManager @@ -53,13 +52,25 @@ else: from queue import Queue + class GQCNNSearch(object): - def __init__(self, analysis_config, train_configs, datasets, split_names, base_models=[], output_dir=None, search_name=None, monitor_cpu=True, monitor_gpu=True, cpu_cores=[], gpu_devices=[]): + def __init__(self, + analysis_config, + train_configs, + datasets, + split_names, + base_models=[], + output_dir=None, + search_name=None, + monitor_cpu=True, + monitor_gpu=True, + cpu_cores=[], + gpu_devices=[]): self._analysis_cfg = analysis_config - + # Create trial output dir if not specified. if search_name is None: - search_name = "gqcnn_hyperparam_search_{}".format(gen_timestamp()) + search_name = "gqcnn_hyperparam_search_{}".format(gen_timestamp()) if output_dir is None: output_dir = "models" self._trial_output_dir = os.path.join(output_dir, search_name) @@ -67,27 +78,61 @@ def __init__(self, analysis_config, train_configs, datasets, split_names, base_m os.makedirs(self._trial_output_dir) # Set up logger. - self._logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(self._trial_output_dir, "search.log"), global_log_file=True) + self._logger = Logger.get_logger(self.__class__.__name__, + log_file=os.path.join( + self._trial_output_dir, + "search.log"), + global_log_file=True) # Init resource manager. - self._resource_manager = ResourceManager(TrialConstants.TRIAL_CPU_LOAD, TrialConstants.TRIAL_GPU_LOAD, TrialConstants.TRIAL_GPU_MEM, monitor_cpu=monitor_cpu, monitor_gpu=monitor_gpu, cpu_cores=cpu_cores, gpu_devices=gpu_devices) - + self._resource_manager = ResourceManager(TrialConstants.TRIAL_CPU_LOAD, + TrialConstants.TRIAL_GPU_LOAD, + TrialConstants.TRIAL_GPU_MEM, + monitor_cpu=monitor_cpu, + monitor_gpu=monitor_gpu, + cpu_cores=cpu_cores, + gpu_devices=gpu_devices) + # Parse train configs and generate individual trial parameters. if len(base_models) > 0: - assert len(train_configs) == len(datasets) == len(split_names) == len(base_models), "Must have equal number of training configs, datasets, split_names, and base models!" + inconsistent_inputs_msg = ("Must have equal number of training" + " configs, datasets, split_names, and" + " base models!") + assert len(train_configs) == len(datasets) == len( + split_names) == len(base_models), inconsistent_inputs_msg else: - assert len(train_configs) == len(datasets) == len(split_names), "Must have equal number of training configs, datasets, and split_names!" + inconsistent_inputs_msg = ("Must have equal number of training" + " configs, datasets, and split_names!") + assert len(train_configs) == len(datasets) == len( + split_names), inconsistent_inputs_msg self._logger.info("Generating trial parameters...") - trial_params = gen_trial_params(train_configs, datasets, split_names, base_models=base_models) + trial_params = gen_trial_params(train_configs, + datasets, + split_names, + base_models=base_models) # Create pending trial queue. self._trials_pending_queue = Queue() if len(base_models) > 0: - for trial_name, hyperparam_summary, train_cfg, dataset, base_model, split_name in trial_params: - self._trials_pending_queue.put(GQCNNFineTuningAndAnalysisTrial(self._analysis_cfg, train_cfg, dataset, base_model, split_name, self._trial_output_dir, trial_name, hyperparam_summary)) + for trial_name, hyperparam_summary, train_cfg, dataset, \ + base_model, split_name in trial_params: + self._trials_pending_queue.put( + GQCNNFineTuningAndAnalysisTrial(self._analysis_cfg, + train_cfg, dataset, + base_model, split_name, + self._trial_output_dir, + trial_name, + hyperparam_summary)) else: - for trial_name, hyperparam_summary, train_cfg, dataset, split_name in trial_params: - self._trials_pending_queue.put(GQCNNTrainingAndAnalysisTrial(self._analysis_cfg, train_cfg, dataset, split_name, self._trial_output_dir, trial_name, hyperparam_summary)) + for trial_name, hyperparam_summary, train_cfg, dataset, \ + split_name in trial_params: + self._trials_pending_queue.put( + GQCNNTrainingAndAnalysisTrial(self._analysis_cfg, + train_cfg, dataset, + split_name, + self._trial_output_dir, + trial_name, + hyperparam_summary)) # Create containers to hold running, finished, and errored-out trials. self._trials_running = [] @@ -98,7 +143,6 @@ def search(self): self._logger.info("Beginning hyper-parameter search...") done = False waiting_for_trial_init = False - delay_resource_check = False last_schedule_attempt_time = -1 search_start_time = time.time() while not done: @@ -107,32 +151,49 @@ def search(self): num_trials_finished = len(self._trials_finished) num_trials_errored = len(self._trials_errored) - self._logger.info("----------------------------------------------------") - self._logger.info("Num trials pending: {}".format(num_trials_pending)) - self._logger.info("Num trials running: {}".format(num_trials_running)) - self._logger.info("Num trials finished: {}".format(num_trials_finished)) + self._logger.info( + "----------------------------------------------------") + self._logger.info( + "Num trials pending: {}".format(num_trials_pending)) + self._logger.info( + "Num trials running: {}".format(num_trials_running)) + self._logger.info( + "Num trials finished: {}".format(num_trials_finished)) if num_trials_errored > 0: - self._logger.info("Num trials errored: {}".format(num_trials_errored)) + self._logger.info( + "Num trials errored: {}".format(num_trials_errored)) - if num_trials_pending > 0 and not waiting_for_trial_init and (time.time() - last_schedule_attempt_time) > SearchConstants.MIN_TIME_BETWEEN_SCHEDULE_ATTEMPTS: + if num_trials_pending > 0 and not waiting_for_trial_init and ( + time.time() - last_schedule_attempt_time + ) > SearchConstants.MIN_TIME_BETWEEN_SCHEDULE_ATTEMPTS: self._logger.info("Attempting to schedule more trials...") - num_trials_to_schedule, gpus_avail = self._resource_manager.num_trials_to_schedule(num_trials_pending) - self._logger.info("Scheduling {} trials".format(num_trials_to_schedule)) + num_trials_to_schedule, gpus_avail = \ + self._resource_manager.num_trials_to_schedule( + num_trials_pending) + self._logger.info( + "Scheduling {} trials".format(num_trials_to_schedule)) if num_trials_to_schedule > 0: # Start trials. - for _, gpu in zip(range(num_trials_to_schedule), gpus_avail): + for _, gpu in zip(range(num_trials_to_schedule), + gpus_avail): trial = self._trials_pending_queue.get() - trial.begin(gpu_avail=gpu, cpu_cores_avail=self._resource_manager.cpu_cores) + trial.begin( + gpu_avail=gpu, + cpu_cores_avail=self._resource_manager.cpu_cores) self._trials_running.append(trial) - # Block scheduling until trials have started training (this is when we know what resources are still available). + # Block scheduling until trials have started training (this + # is when we know what resources are still available). waiting_for_trial_init = True last_schedule_attempt_time = time.time() # Check if trials have started training. if waiting_for_trial_init: - training_has_started = [trial.training_status == GQCNNTrainingStatus.TRAINING for trial in self._trials_running] + training_has_started = [ + trial.training_status == GQCNNTrainingStatus.TRAINING + for trial in self._trials_running + ] if all(training_has_started): waiting_for_trial_init = False @@ -159,10 +220,14 @@ def search(self): done = (num_trials_pending == 0) and (num_trials_running == 0) time.sleep(SearchConstants.SEARCH_THREAD_SLEEP) - self._logger.info("------------------Successful Trials------------------") + self._logger.info( + "------------------Successful Trials------------------") self._logger.info(log_trial_status(self._trials_finished)) if len(self._trials_errored) > 0: - self._logger.info("--------------------Failed Trials--------------------") + self._logger.info( + "--------------------Failed Trials--------------------") self._logger.info(log_trial_status(self._trials_errored)) - self._logger.info("Hyper-parameter search finished in {} seconds.".format(time.time() - search_start_time)) + self._logger.info( + "Hyper-parameter search finished in {} seconds.".format( + time.time() - search_start_time)) diff --git a/gqcnn/search/trial.py b/gqcnn/search/trial.py index 64b96f94..10592a85 100644 --- a/gqcnn/search/trial.py +++ b/gqcnn/search/trial.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Trials for hyper-parameter search. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division @@ -43,14 +46,17 @@ from ..utils import GeneralConstants, GQCNNTrainingStatus from ..analysis import GQCNNAnalyzer + class TrialStatus: PENDING = "pending" RUNNING = "running" FINISHED = "finished" EXCEPTION = "exception" + class GQCNNTrialWithAnalysis(with_metaclass(ABCMeta, object)): - def __init__(self, analysis_cfg, train_cfg, dataset_dir, split_name, output_dir, model_name, hyperparam_summary): + def __init__(self, analysis_cfg, train_cfg, dataset_dir, split_name, + output_dir, model_name, hyperparam_summary): self._analysis_cfg = analysis_cfg self._train_cfg = train_cfg self._dataset_dir = dataset_dir @@ -59,12 +65,17 @@ def __init__(self, analysis_cfg, train_cfg, dataset_dir, split_name, output_dir, self._model_name = model_name self._hyperparam_summary = hyperparam_summary self._manager = multiprocessing.Manager() - self._train_progress_dict = self._build_train_progress_dict() # To communicate with training. - self._trial_progress_dict = self._build_trial_progress_dict() # To communicate with trial. + # To communicate with training. + self._train_progress_dict = self._build_train_progress_dict() + # To communicate with trial. + self._trial_progress_dict = self._build_trial_progress_dict() self._process = None def _build_train_progress_dict(self): - progress_dict = self._manager.dict(training_status=GQCNNTrainingStatus.NOT_STARTED, epoch=np.nan, analysis=None) + progress_dict = self._manager.dict( + training_status=GQCNNTrainingStatus.NOT_STARTED, + epoch=np.nan, + analysis=None) return progress_dict def _build_trial_progress_dict(self): @@ -75,29 +86,52 @@ def _build_trial_progress_dict(self): def _run(self, trainer): pass - def _run_trial(self, analysis_config, train_config, dataset_dir, split_name, output_dir, model_name, train_progress_dict, trial_progress_dict, hyperparam_summary, gpu_avail="", cpu_cores_avail=[], backend="tf"): + def _run_trial(self, + analysis_config, + train_config, + dataset_dir, + split_name, + output_dir, + model_name, + train_progress_dict, + trial_progress_dict, + hyperparam_summary, + gpu_avail="", + cpu_cores_avail=[], + backend="tf"): trial_progress_dict["status"] = TrialStatus.RUNNING try: - os.system("taskset -pc {} {}".format(",".join(str(i) for i in cpu_cores_avail), os.getpid())) + os.system("taskset -pc {} {}".format( + ",".join(str(i) for i in cpu_cores_avail), os.getpid())) os.environ["CUDA_VISIBLE_DEVICES"] = gpu_avail - gqcnn = get_gqcnn_model(backend, verbose=False)(train_config["gqcnn"], verbose=False) - trainer = get_gqcnn_trainer(backend)(gqcnn, - dataset_dir, - split_name, - output_dir, - train_config, - name=model_name, - progress_dict=train_progress_dict, - verbose=False) + gqcnn = get_gqcnn_model(backend, + verbose=False)(train_config["gqcnn"], + verbose=False) + trainer = get_gqcnn_trainer(backend)( + gqcnn, + dataset_dir, + split_name, + output_dir, + train_config, + name=model_name, + progress_dict=train_progress_dict, + verbose=False) self._run(trainer) - with open(os.path.join(output_dir, model_name, "hyperparam_summary.json"), "wb") as fhandle: - json.dump(hyperparam_summary, fhandle, indent=GeneralConstants.JSON_INDENT) - + with open( + os.path.join(output_dir, model_name, + "hyperparam_summary.json"), "wb") as fhandle: + json.dump(hyperparam_summary, + fhandle, + indent=GeneralConstants.JSON_INDENT) + train_progress_dict["training_status"] = "analyzing" analyzer = GQCNNAnalyzer(analysis_config, verbose=False) - _, _, init_train_error, final_train_error, init_train_loss, final_train_loss, init_val_error, final_val_error, norm_final_val_error = analyzer.analyze(os.path.join(output_dir, model_name), output_dir) + _, _, init_train_error, final_train_error, init_train_loss, \ + final_train_loss, init_val_error, final_val_error, \ + norm_final_val_error = analyzer.analyze( + os.path.join(output_dir, model_name), output_dir) analysis_dict = {} analysis_dict["init_train_error"] = init_train_error analysis_dict["final_train_error"] = final_train_error @@ -134,27 +168,52 @@ def training_status(self): def begin(self, gpu_avail="", cpu_cores_avail=[]): self._status = TrialStatus.RUNNING - self._process = multiprocessing.Process(target=self._run_trial, args=(self._analysis_cfg, self._train_cfg, self._dataset_dir, self._split_name, self._output_dir, self._model_name, self._train_progress_dict, self._trial_progress_dict, self._hyperparam_summary, gpu_avail, cpu_cores_avail)) + self._process = multiprocessing.Process( + target=self._run_trial, + args=(self._analysis_cfg, self._train_cfg, self._dataset_dir, + self._split_name, self._output_dir, self._model_name, + self._train_progress_dict, self._trial_progress_dict, + self._hyperparam_summary, gpu_avail, cpu_cores_avail)) self._process.start() def __str__(self): - trial_str = "Trial: {}, Training Stage: {}".format(self._model_name, self.training_status) - if self.training_status == GQCNNTrainingStatus.TRAINING and not np.isnan(self._train_progress_dict["epoch"]): - trial_str += ", Epoch: {}/{}".format(self._train_progress_dict["epoch"], self._train_cfg["num_epochs"]) + trial_str = "Trial: {}, Training Stage: {}".format( + self._model_name, self.training_status) + if self.training_status == GQCNNTrainingStatus.TRAINING and not \ + np.isnan(self._train_progress_dict["epoch"]): + trial_str += ", Epoch: {}/{}".format( + self._train_progress_dict["epoch"], + self._train_cfg["num_epochs"]) if self.errored_out: trial_str += ", Error message: {}".format(self.error_msg) if self.training_status == "finished": - trial_str += ", Initial train error: {}, Final train error: {}, Initial train loss: {}, Final train loss: {}, Initial val error: {}, Final val error: {}, Norm final val error: {}".format(self._train_progress_dict["analysis"]["init_train_error"], self._train_progress_dict["analysis"]["final_train_error"], self._train_progress_dict["analysis"]["init_train_loss"], self._train_progress_dict["analysis"]["final_train_loss"], self._train_progress_dict["analysis"]["init_val_error"], self._train_progress_dict["analysis"]["final_val_error"], self._train_progress_dict["analysis"]["norm_final_val_error"]) + finished_msg = (", Initial train error: {}, Final train error: {}," + " Initial train loss: {}, Final train loss: {}," + " Initial val error: {}, Final val error: {}, Norm" + " final val error: {}") + trial_str += finished_msg.format( + self._train_progress_dict["analysis"]["init_train_error"], + self._train_progress_dict["analysis"]["final_train_error"], + self._train_progress_dict["analysis"]["init_train_loss"], + self._train_progress_dict["analysis"]["final_train_loss"], + self._train_progress_dict["analysis"]["init_val_error"], + self._train_progress_dict["analysis"]["final_val_error"], + self._train_progress_dict["analysis"]["norm_final_val_error"]) return trial_str + class GQCNNTrainingAndAnalysisTrial(GQCNNTrialWithAnalysis): def _run(self, trainer): trainer.train() + class GQCNNFineTuningAndAnalysisTrial(GQCNNTrialWithAnalysis): - def __init__(self, analysis_cfg, train_cfg, dataset_dir, base_model_dir, split_name, output_dir, model_name, hyperparam_summary): - GQCNNTrialWithAnalysis.__init__(self, analysis_cfg, train_cfg, dataset_dir, split_name, output_dir, model_name, hyperparam_summary) - + def __init__(self, analysis_cfg, train_cfg, dataset_dir, base_model_dir, + split_name, output_dir, model_name, hyperparam_summary): + GQCNNTrialWithAnalysis.__init__(self, analysis_cfg, train_cfg, + dataset_dir, split_name, output_dir, + model_name, hyperparam_summary) + self._base_model_dir = base_model_dir def _run(self, trainer): diff --git a/gqcnn/search/utils.py b/gqcnn/search/utils.py index bd30d1c3..40e6dfbf 100644 --- a/gqcnn/search/utils.py +++ b/gqcnn/search/utils.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Utility functions for hyper-parameter search. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division @@ -34,9 +37,6 @@ from datetime import datetime import itertools -import numpy as np - -from ..utils import GQCNNTrainingStatus def get_fields_to_search_over(train_config, prev_keys=[]): fields = [] @@ -45,7 +45,8 @@ def get_fields_to_search_over(train_config, prev_keys=[]): if isinstance(train_config[key], list): prev_keys_copy = copy.deepcopy(prev_keys) prev_keys_copy.append(key) - if isinstance(train_config[key][0], str) and train_config[key][0].startswith("anchor_"): + if isinstance(train_config[key][0], + str) and train_config[key][0].startswith("anchor_"): anchored_fields[train_config[key][0]].append(prev_keys_copy) train_config[key] = train_config[key][1:] else: @@ -53,11 +54,13 @@ def get_fields_to_search_over(train_config, prev_keys=[]): elif isinstance(train_config[key], OrderedDict): prev_keys_copy = copy.deepcopy(prev_keys) prev_keys_copy.append(key) - sub_fields, sub_anchored_fields = get_fields_to_search_over(train_config[key], prev_keys=prev_keys_copy) + sub_fields, sub_anchored_fields = get_fields_to_search_over( + train_config[key], prev_keys=prev_keys_copy) fields.extend(sub_fields) update_dict(anchored_fields, sub_anchored_fields) return fields, anchored_fields + def update_dict(dict1, dict2): for key, val in dict2.items(): if key in dict1: @@ -65,55 +68,70 @@ def update_dict(dict1, dict2): else: dict1[key] = val + def get_nested_key(cfg, key): - val = cfg + val = cfg for k in key: val = val[k] return val + def set_nested_key(cfg, key, val): root_field = cfg for k in key[:-1]: root_field = root_field[k] root_field[key[-1]] = val + def gen_config_summary_dict(hyperparam_combination): summary_dict = {} for key, val in hyperparam_combination: summary_dict["/".join(key)] = val return summary_dict + def parse_master_train_config(train_config): configs = [] - hyperparam_search_fields, hyperparam_anchored_search_fields = get_fields_to_search_over(train_config) + hyperparam_search_fields, hyperparam_anchored_search_fields = \ + get_fields_to_search_over(train_config) - # Ensure a one-to-one mapping between hyperparameters of fields with matching anchor tags. + # Ensure a one-to-one mapping between hyperparameters of fields with + # matching anchor tags. for anchor_tag, fields in hyperparam_anchored_search_fields.items(): num_params = [] for field in fields: num_params.append(len(get_nested_key(train_config, field))) - assert max(num_params) == min(num_params), "All fields in anchor tag \"{}\" do not have the same # of parameters to search over!".format(anchor_tag) + invalid_anchor_tag_msg = ("All fields in anchor tag \"{}\" do not have" + " the same # of parameters to search over!") + assert max(num_params) == min( + num_params), invalid_anchor_tag_msg.format(anchor_tag) # If there is nothing to search over just return the given config. - if len(hyperparam_search_fields) == 0 and len(hyperparam_anchored_search_fields) == 0: + if len(hyperparam_search_fields) == 0 and len( + hyperparam_anchored_search_fields) == 0: return [("", train_config)] # Generate a list of all the possible hyper-parameters to search over. + # Normal fields. hyperparam_search_params = [] - for search_field in hyperparam_search_fields: # Normal fields. + for search_field in hyperparam_search_fields: search_field_params = [] for val in get_nested_key(train_config, search_field): search_field_params.append((search_field, val)) hyperparam_search_params.append(search_field_params) - for anchored_fields in hyperparam_anchored_search_fields.values(): # Anchored fields. - combinations = [[] for _ in range(len(get_nested_key(train_config, anchored_fields[0])))] + # Anchored fields. + for anchored_fields in hyperparam_anchored_search_fields.values(): + combinations = [[] for _ in range( + len(get_nested_key(train_config, anchored_fields[0])))] for field in anchored_fields: for idx, val in enumerate(get_nested_key(train_config, field)): combinations[idx].append((field, val)) hyperparam_search_params.append(combinations) # Get all permutations of the possible hyper-parameters. - hyperparam_combinations = list(itertools.product(*hyperparam_search_params)) + hyperparam_combinations = list( + itertools.product(*hyperparam_search_params)) + def flatten_combo(combo): flattened = [] for item in combo: @@ -123,7 +141,10 @@ def flatten_combo(combo): else: flattened.append(item) return flattened - hyperparam_combinations = [flatten_combo(combo) for combo in hyperparam_combinations] + + hyperparam_combinations = [ + flatten_combo(combo) for combo in hyperparam_combinations + ] # Generate possible configs to search over. for combo in hyperparam_combinations: @@ -133,37 +154,56 @@ def flatten_combo(combo): configs.append((gen_config_summary_dict(combo), config)) return configs + def gen_timestamp(): return str(datetime.now()).split(".")[0].replace(" ", "_") + def gen_trial_params_train(master_train_configs, datasets, split_names): trial_params = [] - for master_train_config, dataset, split_name in zip(master_train_configs, datasets, split_names): + for master_train_config, dataset, split_name in zip( + master_train_configs, datasets, split_names): train_configs = parse_master_train_config(master_train_config) - for i, (hyperparam_summary_dict, train_config) in enumerate(train_configs): - trial_name = "{}_{}_trial_{}_{}".format(dataset.split("/")[-3], split_name, i, gen_timestamp()) - trial_params.append((trial_name, hyperparam_summary_dict, train_config, dataset, split_name)) + for i, (hyperparam_summary_dict, + train_config) in enumerate(train_configs): + trial_name = "{}_{}_trial_{}_{}".format( + dataset.split("/")[-3], split_name, i, gen_timestamp()) + trial_params.append((trial_name, hyperparam_summary_dict, + train_config, dataset, split_name)) return trial_params -def gen_trial_params_finetune(master_train_configs, datasets, base_models, split_names): + +def gen_trial_params_finetune(master_train_configs, datasets, base_models, + split_names): trial_params = [] - for master_train_config, dataset, base_model, split_name in zip(master_train_configs, datasets, base_models, split_names): + for master_train_config, dataset, base_model, split_name in zip( + master_train_configs, datasets, base_models, split_names): train_configs = parse_master_train_config(master_train_config) - for i, (hyperparam_summary_dict, train_config) in enumerate(train_configs): - trial_name = "{}_{}_trial_{}_{}".format(dataset.split("/")[-3], split_name, i, gen_timestamp()) - trial_params.append((trial_name, hyperparam_summary_dict, train_config, dataset, base_model, split_name)) + for i, (hyperparam_summary_dict, + train_config) in enumerate(train_configs): + trial_name = "{}_{}_trial_{}_{}".format( + dataset.split("/")[-3], split_name, i, gen_timestamp()) + trial_params.append( + (trial_name, hyperparam_summary_dict, train_config, dataset, + base_model, split_name)) return trial_params -def gen_trial_params(master_train_configs, datasets, split_names, base_models=[]): +def gen_trial_params(master_train_configs, + datasets, + split_names, + base_models=[]): if len(base_models) > 0: - return gen_trial_params_finetune(master_train_configs, datasets, base_models, split_names) + return gen_trial_params_finetune(master_train_configs, datasets, + base_models, split_names) else: - return gen_trial_params_train(master_train_configs, datasets, split_names) + return gen_trial_params_train(master_train_configs, datasets, + split_names) + def log_trial_status(trials): - status_str = "--------------------TRIAL STATUS--------------------" - for trial in trials: - status_str += "\n" - status_str += "[{}]".format(str(trial)) - return status_str + status_str = "--------------------TRIAL STATUS--------------------" + for trial in trials: + status_str += "\n" + status_str += "[{}]".format(str(trial)) + return status_str diff --git a/gqcnn/training/__init__.py b/gqcnn/training/__init__.py index 0010fafd..bfa5cc00 100644 --- a/gqcnn/training/__init__.py +++ b/gqcnn/training/__init__.py @@ -30,11 +30,11 @@ from __future__ import division from __future__ import print_function -from .tf import * +from .tf import GQCNNTrainerTF + def get_gqcnn_trainer(backend="tf"): - """ - Get the GQ-CNN Trainer for the provided backend. + """Get the GQ-CNN Trainer for the provided backend. Note ---- @@ -43,12 +43,12 @@ def get_gqcnn_trainer(backend="tf"): Parameters ---------- backend : str - the backend to use, currently only 'tf' is supported + The backend to use, currently only "tf" is supported. Returns ------- :obj:`gqcnn.training.tf.GQCNNTrainerTF` - GQ-CNN Trainer with TensorFlow backend + GQ-CNN Trainer with TensorFlow backend. """ # Return desired `GQCNNTrainer` instance based on backend. if backend == "tf": diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index bae8f312..55b24bf8 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -23,16 +23,16 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Trains a GQ-CNN network using Tensorflow backend. -Author: Vishal Satish and Jeff Mahler + +Author +------ +Vishal Satish & Jeff Mahler """ from __future__ import absolute_import from __future__ import division from __future__ import print_function -import argparse import collections -import copy -import pickle as pkl import json import multiprocessing as mp import os @@ -41,9 +41,9 @@ import signal import subprocess import sys -import threading import time +from past.builtins import xrange import cv2 import numpy as np import scipy.misc as sm @@ -51,24 +51,26 @@ import tensorflow as tf from autolab_core import (BinaryClassificationResult, RegressionResult, - TensorDataset, YamlConfig, Logger) -from autolab_core.constants import * + TensorDataset, Logger) +from autolab_core.constants import JSON_INDENT import autolab_core.utils as utils -from ...utils import (ImageMode, TrainingMode, GripperMode, - InputDepthMode, GeneralConstants, TrainStatsLogger, - pose_dim, read_pose_data, weight_name_to_layer_name, - is_py2, GQCNNTrainingStatus, GQCNNFilenames) +from ...utils import (TrainingMode, GripperMode, InputDepthMode, + GeneralConstants, TrainStatsLogger, pose_dim, + read_pose_data, weight_name_to_layer_name, is_py2, + GQCNNTrainingStatus, GQCNNFilenames) if is_py2(): import Queue else: import queue as Queue + class GQCNNTrainerTF(object): """Trains a GQ-CNN with Tensorflow backend.""" - def __init__(self, gqcnn, + def __init__(self, + gqcnn, dataset_dir, split_name, output_dir, @@ -80,17 +82,17 @@ def __init__(self, gqcnn, Parameters ---------- gqcnn : :obj:`GQCNN` - grasp quality neural network to optimize + Grasp quality neural network to optimize. dataset_dir : str - path to the training/validation dataset + Path to the training/validation dataset. split_name : str - name of the split to train on + Name of the split to train on. output_dir : str - path to save the model output + Path to save the model output. config : dict - dictionary of configuration parameters + Dictionary of configuration parameters. name : str - name of the the model + Name of the the model. """ self.gqcnn = gqcnn self.dataset_dir = dataset_dir @@ -111,39 +113,58 @@ def __init__(self, gqcnn, os.mkdir(self.model_dir) # Set up logger. - self.logger = Logger.get_logger(self.__class__.__name__, log_file=os.path.join(self.model_dir, "training.log"), silence=(not verbose), global_log_file=verbose) + self.logger = Logger.get_logger(self.__class__.__name__, + log_file=os.path.join( + self.model_dir, "training.log"), + silence=(not verbose), + global_log_file=verbose) # Check default split. if split_name is None: self.logger.warning("Using default image-wise split.") self.split_name = "image_wise" - + # Update cfg for saving. self.cfg["dataset_dir"] = self.dataset_dir self.cfg["split_name"] = self.split_name - + def _create_loss(self): """Creates a loss based on config file. Returns ------- :obj:`tensorflow Tensor` - loss + Loss. """ if self.cfg["loss"] == "l2": - return (1.0 / self.train_batch_size) * tf.nn.l2_loss(tf.subtract(tf.nn.sigmoid(self.train_net_output), self.train_labels_node)) + return (1.0 / self.train_batch_size) * tf.nn.l2_loss( + tf.subtract(tf.nn.sigmoid(self.train_net_output), + self.train_labels_node)) elif self.cfg["loss"] == "sparse": if self._angular_bins > 0: - log = tf.reshape(tf.dynamic_partition(self.train_net_output, self.train_pred_mask_node, 2)[1], (-1, 2)) - return tf.reduce_mean(tf.nn.sparse_softmax_cross_entropy_with_logits(_sentinel=None, labels=self.train_labels_node, - logits=log)) + log = tf.reshape( + tf.dynamic_partition(self.train_net_output, + self.train_pred_mask_node, 2)[1], + (-1, 2)) + return tf.reduce_mean( + tf.nn.sparse_softmax_cross_entropy_with_logits( + _sentinel=None, + labels=self.train_labels_node, + logits=log)) else: - return tf.reduce_mean(tf.nn.sparse_softmax_cross_entropy_with_logits(_sentinel=None, labels=self.train_labels_node, logits=self.train_net_output, name=None)) + return tf.reduce_mean( + tf.nn.sparse_softmax_cross_entropy_with_logits( + _sentinel=None, + labels=self.train_labels_node, + logits=self.train_net_output, + name=None)) elif self.cfg["loss"] == "weighted_cross_entropy": - return tf.reduce_mean(tf.nn.weighted_cross_entropy_with_logits(targets=tf.reshape(self.train_labels_node, [-1,1]), - logits=self.train_net_output, - pos_weight=self.pos_weight, - name=None)) + return tf.reduce_mean( + tf.nn.weighted_cross_entropy_with_logits( + targets=tf.reshape(self.train_labels_node, [-1, 1]), + logits=self.train_net_output, + pos_weight=self.pos_weight, + name=None)) def _create_optimizer(self, loss, batch, var_list, learning_rate): """Create optimizer based on config file. @@ -151,92 +172,104 @@ def _create_optimizer(self, loss, batch, var_list, learning_rate): Parameters ---------- loss : :obj:`tensorflow Tensor` - loss to use, generated with _create_loss() + Loss to use, generated with `_create_loss`. batch : :obj:`tf.Variable` - variable to keep track of the current gradient step number + Variable to keep track of the current gradient step number. var_list : :obj:`lst` - list of tf.Variable objects to update to minimize loss(ex. network weights) + List of tf.Variable objects to update to minimize loss (ex. network + weights). learning_rate : float - learning rate for training + Learning rate for training. Returns ------- :obj:`tf.train.Optimizer` - optimizer - """ + Optimizer. + """ # Instantiate optimizer. if self.cfg["optimizer"] == "momentum": - optimizer = tf.train.MomentumOptimizer(learning_rate, self.momentum_rate) + optimizer = tf.train.MomentumOptimizer(learning_rate, + self.momentum_rate) elif self.cfg["optimizer"] == "adam": optimizer = tf.train.AdamOptimizer(learning_rate) elif self.cfg["optimizer"] == "rmsprop": optimizer = tf.train.RMSPropOptimizer(learning_rate) else: - raise ValueError("Optimizer \"{}\" not supported".format(self.cfg["optimizer"])) + raise ValueError("Optimizer \"{}\" not supported".format( + self.cfg["optimizer"])) # Compute gradients. - gradients, variables = zip(*optimizer.compute_gradients(loss, var_list=var_list)) + gradients, variables = zip( + *optimizer.compute_gradients(loss, var_list=var_list)) # Clip gradients to prevent exploding gradient problem. - gradients, global_grad_norm = tf.clip_by_global_norm(gradients, self.max_global_grad_norm) + gradients, global_grad_norm = tf.clip_by_global_norm( + gradients, self.max_global_grad_norm) # Generate op to apply gradients. - apply_grads = optimizer.apply_gradients(zip(gradients, variables), global_step=batch) + apply_grads = optimizer.apply_gradients(zip(gradients, variables), + global_step=batch) return apply_grads, global_grad_norm def _launch_tensorboard(self): """Launches Tensorboard to visualize training.""" FNULL = open(os.devnull, "w") - self.logger.info( - "Launching Tensorboard, Please navigate to localhost:{} in your favorite web browser to view summaries".format(self._tensorboard_port)) - self._tensorboard_proc = subprocess.Popen(["tensorboard", "--port", str(self._tensorboard_port),"--logdir", self.summary_dir], stdout=FNULL) + tensorboard_launch_msg = ("Launching Tensorboard, please navigate to" + " localhost:{} in your favorite web browser" + " to view summaries.") + self.logger.info(tensorboard_launch_msg.format(self._tensorboard_port)) + self._tensorboard_proc = subprocess.Popen([ + "tensorboard", "--port", + str(self._tensorboard_port), "--logdir", self.summary_dir + ], + stdout=FNULL) def _close_tensorboard(self): """Closes Tensorboard process.""" self.logger.info("Closing Tensorboard...") - self._tensorboard_proc.terminate() + self._tensorboard_proc.terminate() def train(self): """Perform optimization.""" with self.gqcnn.tf_graph.as_default(): self._train() - + def _train(self): """Perform optimization.""" - start_time = time.time() - - # Run setup. + # Run setup. if self.progress_dict is not None: - self.progress_dict["training_status"] = GQCNNTrainingStatus.SETTING_UP + self.progress_dict[ + "training_status"] = GQCNNTrainingStatus.SETTING_UP self._setup() - + # Build network. self.gqcnn.initialize_network(self.input_im_node, self.input_pose_node) # Optimize weights. if self.progress_dict is not None: - self.progress_dict["training_status"] = GQCNNTrainingStatus.TRAINING + self.progress_dict[ + "training_status"] = GQCNNTrainingStatus.TRAINING self._optimize_weights() def finetune(self, base_model_dir): """Perform fine-tuning. - + Parameters ---------- base_model_dir : str - path to the pre-trained base model to use + Path to the pre-trained base model to use. """ with self.gqcnn.tf_graph.as_default(): self._finetune(base_model_dir) - + def _finetune(self, base_model_dir): """Perform fine-tuning. - + Parameters ---------- base_model_dir : str - path to the pre-trained base model to use + Path to the pre-trained base model to use. """ # Set flag and base model for fine-tuning. self.finetuning = True @@ -244,16 +277,17 @@ def _finetune(self, base_model_dir): # Run setup. self._setup() - + # Build network. self.gqcnn.set_base_network(base_model_dir) self.gqcnn.initialize_network(self.input_im_node, self.input_pose_node) - + # Optimize weights. if self.progress_dict is not None: - self.progress_dict["training_status"] = GQCNNTrainingStatus.TRAINING + self.progress_dict[ + "training_status"] = GQCNNTrainingStatus.TRAINING self._optimize_weights(finetune=True) - + def _optimize_weights(self, finetune=False): """Optimize the network weights.""" start_time = time.time() @@ -268,11 +302,12 @@ def _optimize_weights(self, finetune=False): elif self.training_mode == TrainingMode.REGRESSION: self.gqcnn.add_sigmoid_to_output() else: - raise ValueError("Training mode: {} not supported !".format(self.training_mode)) + raise ValueError("Training mode: {} not supported !".format( + self.training_mode)) train_predictions = self.gqcnn.output drop_rate_in = self.gqcnn.input_drop_rate_node self.weights = self.gqcnn.weights - + # Once weights have been initialized, create TF saver for weights. self.saver = tf.train.Saver() @@ -281,7 +316,7 @@ def _optimize_weights(self, finetune=False): # Part 1: error. loss = self._create_loss() unregularized_loss = loss - + # Part 2: regularization. layer_weights = list(self.weights.values()) with tf.name_scope("regularization"): @@ -293,10 +328,10 @@ def _optimize_weights(self, finetune=False): # Setup learning rate. batch = tf.Variable(0) learning_rate = tf.train.exponential_decay( - self.base_lr, # Base learning rate. + self.base_lr, # Base learning rate. batch * self.train_batch_size, # Current index into the dataset. - self.decay_step, # Decay step. - self.decay_rate, # decay rate. + self.decay_step, # Decay step. + self.decay_rate, # decay rate. staircase=True) # Setup variable list. @@ -305,21 +340,25 @@ def _optimize_weights(self, finetune=False): var_list = [] for weights_name, weights_val in self.weights.items(): layer_name = weight_name_to_layer_name(weights_name) - if self.optimize_base_layers or layer_name not in self.gqcnn._base_layer_names: + if self.optimize_base_layers or \ + layer_name not in self.gqcnn._base_layer_names: var_list.append(weights_val) # Create optimizer. with tf.name_scope("optimizer"): - apply_grad_op, global_grad_norm = self._create_optimizer(loss, batch, var_list, learning_rate) + apply_grad_op, global_grad_norm = self._create_optimizer( + loss, batch, var_list, learning_rate) # Add a handler for SIGINT for graceful exit. def handler(signum, frame): self.logger.info("caught CTRL+C, exiting...") self._cleanup() exit(0) + signal.signal(signal.SIGINT, handler) - # Now that everything in our graph is set up, we write the graph to the summary event so it can be visualized in Tensorboard. + # Now that everything in our graph is set up, we write the graph to the + # summary event so it can be visualized in Tensorboard. self.summary_writer.add_graph(self.gqcnn.tf_graph) # Begin optimization loop. @@ -330,7 +369,7 @@ def handler(signum, frame): for i in range(self.num_prefetch_q_workers): if self.num_prefetch_q_workers > 1 or not self._debug: seed = np.random.randint(GeneralConstants.SEED_SAMPLE_MAX) - p = mp.Process(target=self._load_and_enqueue, args=(seed,)) + p = mp.Process(target=self._load_and_enqueue, args=(seed, )) p.start() self.prefetch_q_workers.append(p) @@ -340,103 +379,198 @@ def handler(signum, frame): self.logger.info("Beginning Optimization...") - # Create a `TrainStatsLogger` object to log training statistics at certain intervals. + # Create a `TrainStatsLogger` object to log training statistics at + # certain intervals. self.train_stats_logger = TrainStatsLogger(self.model_dir) # Loop through training steps. - training_range = xrange(int(self.num_epochs * self.num_train) // self.train_batch_size) + training_range = xrange( + int(self.num_epochs * self.num_train) // self.train_batch_size) for step in training_range: # Run optimization. step_start = time.time() if self._angular_bins > 0: images, poses, labels, masks = self.prefetch_q.get() - _, l, ur_l, lr, predictions, raw_net_output = self.sess.run([apply_grad_op, loss, unregularized_loss, learning_rate, train_predictions, self.train_net_output], feed_dict={drop_rate_in: self.drop_rate, self.input_im_node: images, self.input_pose_node: poses, self.train_labels_node: labels, self.train_pred_mask_node: masks}, options=GeneralConstants.timeout_option) + _, l, ur_l, lr, predictions, raw_net_output = \ + self.sess.run( + [ + apply_grad_op, loss, unregularized_loss, + learning_rate, train_predictions, + self.train_net_output + ], + feed_dict={ + drop_rate_in: self.drop_rate, + self.input_im_node: images, + self.input_pose_node: poses, + self.train_labels_node: labels, + self.train_pred_mask_node: masks + }, + options=GeneralConstants.timeout_option) else: images, poses, labels = self.prefetch_q.get() - _, l, ur_l, lr, predictions, raw_net_output = self.sess.run([apply_grad_op, loss, unregularized_loss, learning_rate, train_predictions, self.train_net_output], feed_dict={drop_rate_in: self.drop_rate, self.input_im_node: images, self.input_pose_node: poses, self.train_labels_node: labels}, options=GeneralConstants.timeout_option) + _, l, ur_l, lr, predictions, raw_net_output = \ + self.sess.run( + [ + apply_grad_op, loss, unregularized_loss, + learning_rate, train_predictions, + self.train_net_output + ], + feed_dict={ + drop_rate_in: self.drop_rate, + self.input_im_node: images, + self.input_pose_node: poses, + self.train_labels_node: labels + }, + options=GeneralConstants.timeout_option) step_stop = time.time() - self.logger.info("Step took {} sec.".format(round(step_stop-step_start, 3))) - + self.logger.info("Step took {} sec.".format( + round(step_stop - step_start, 3))) + if self.training_mode == TrainingMode.REGRESSION: - self.logger.info("Max " + str(np.max(predictions))) + self.logger.info("Max " + str(np.max(predictions))) self.logger.info("Min " + str(np.min(predictions))) elif self.cfg["loss"] != "weighted_cross_entropy": if self._angular_bins == 0: - ex = np.exp(raw_net_output - np.tile(np.max(raw_net_output, axis=1)[:,np.newaxis], [1,2])) - softmax = ex / np.tile(np.sum(ex, axis=1)[:,np.newaxis], [1,2]) - - self.logger.info("Max " + str(np.max(softmax[:,1]))) - self.logger.info("Min " + str(np.min(softmax[:,1]))) - self.logger.info("Pred nonzero " + str(np.sum(softmax[:,1] > 0.5))) + ex = np.exp(raw_net_output - np.tile( + np.max(raw_net_output, axis=1)[:, np.newaxis], + [1, 2])) + softmax = ex / np.tile( + np.sum(ex, axis=1)[:, np.newaxis], [1, 2]) + + self.logger.info("Max " + str(np.max(softmax[:, 1]))) + self.logger.info("Min " + str(np.min(softmax[:, 1]))) + self.logger.info("Pred nonzero " + + str(np.sum(softmax[:, 1] > 0.5))) self.logger.info("True nonzero " + str(np.sum(labels))) - + else: sigmoid = 1.0 / (1.0 + np.exp(-raw_net_output)) - self.logger.info("Max " + str(np.max(sigmoid))) + self.logger.info("Max " + str(np.max(sigmoid))) self.logger.info("Min " + str(np.min(sigmoid))) - self.logger.info("Pred nonzero " + str(np.sum(sigmoid > 0.5))) - self.logger.info("True nonzero " + str(np.sum(labels > 0.5))) + self.logger.info("Pred nonzero " + + str(np.sum(sigmoid > 0.5))) + self.logger.info("True nonzero " + + str(np.sum(labels > 0.5))) if np.isnan(l) or np.any(np.isnan(poses)): - self.logger.error("Encountered NaN in loss or training poses!") + self.logger.error( + "Encountered NaN in loss or training poses!") raise Exception - + # Log output. if step % self.log_frequency == 0: elapsed_time = time.time() - start_time start_time = time.time() self.logger.info("Step {} (epoch {}), {} s".format( - step, round(step * self.train_batch_size / self.num_train, 3), - round(1000 * elapsed_time / self.eval_frequency, 2))) - self.logger.info("Minibatch loss: {}, learning rate: {}".format(round(l, 3), round(lr, 6))) + step, + round(step * self.train_batch_size / self.num_train, + 3), + round(1000 * elapsed_time / self.eval_frequency, 2))) + self.logger.info( + "Minibatch loss: {}, learning rate: {}".format( + round(l, 3), round(lr, 6))) if self.progress_dict is not None: - self.progress_dict["epoch"] = round(step * self.train_batch_size / self.num_train, 2) + self.progress_dict["epoch"] = round( + step * self.train_batch_size / self.num_train, 2) train_error = l if self.training_mode == TrainingMode.CLASSIFICATION: if self._angular_bins > 0: - predictions = predictions[masks.astype(bool)].reshape((-1, 2)) - classification_result = BinaryClassificationResult(predictions[:,1], labels) + predictions = predictions[masks.astype( + bool)].reshape((-1, 2)) + classification_result = BinaryClassificationResult( + predictions[:, 1], labels) train_error = classification_result.error_rate - - self.logger.info("Minibatch error: {}".format(round(train_error, 3))) - - self.summary_writer.add_summary(self.sess.run(self.merged_log_summaries, feed_dict={self.minibatch_error_placeholder: train_error, self.minibatch_loss_placeholder: l, self.learning_rate_placeholder: lr}), step) + + self.logger.info("Minibatch error: {}".format( + round(train_error, 3))) + + self.summary_writer.add_summary( + self.sess.run( + self.merged_log_summaries, + feed_dict={ + self.minibatch_error_placeholder: train_error, + self.minibatch_loss_placeholder: l, + self.learning_rate_placeholder: lr + }), step) sys.stdout.flush() # Update the `TrainStatsLogger`. - self.train_stats_logger.update(train_eval_iter=step, train_loss=l, train_error=train_error, total_train_error=None, val_eval_iter=None, val_error=None, learning_rate=lr) + self.train_stats_logger.update(train_eval_iter=step, + train_loss=l, + train_error=train_error, + total_train_error=None, + val_eval_iter=None, + val_error=None, + learning_rate=lr) # Evaluate model. if step % self.eval_frequency == 0 and step > 0: if self.cfg["eval_total_train_error"]: - train_result = self._error_rate_in_batches(validation_set=False) - self.logger.info("Training error: {}".format(round(train_result.error_rate, 3))) + train_result = self._error_rate_in_batches( + validation_set=False) + self.logger.info("Training error: {}".format( + round(train_result.error_rate, 3))) # Update the `TrainStatsLogger` and save. - self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=train_result.error_rate, total_train_loss=train_result.cross_entropy_loss, val_eval_iter=None, val_error=None, learning_rate=None) + self.train_stats_logger.update( + train_eval_iter=None, + train_loss=None, + train_error=None, + total_train_error=train_result.error_rate, + total_train_loss=train_result.cross_entropy_loss, + val_eval_iter=None, + val_error=None, + learning_rate=None) self.train_stats_logger.log() - + if self.train_pct < 1.0: val_result = self._error_rate_in_batches() - self.summary_writer.add_summary(self.sess.run(self.merged_eval_summaries, feed_dict={self.val_error_placeholder: val_result.error_rate}), step) - self.logger.info("Validation error: {}".format(round(val_result.error_rate, 3))) - self.logger.info("Validation loss: {}".format(round(val_result.cross_entropy_loss, 3))) + self.summary_writer.add_summary( + self.sess.run( + self.merged_eval_summaries, + feed_dict={ + self.val_error_placeholder: val_result. + error_rate + }), step) + self.logger.info("Validation error: {}".format( + round(val_result.error_rate, 3))) + self.logger.info("Validation loss: {}".format( + round(val_result.cross_entropy_loss, 3))) sys.stdout.flush() # Update the `TrainStatsLogger`. if self.train_pct < 1.0: - self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=None, val_eval_iter=step, val_loss=val_result.cross_entropy_loss, val_error=val_result.error_rate, learning_rate=None) + self.train_stats_logger.update( + train_eval_iter=None, + train_loss=None, + train_error=None, + total_train_error=None, + val_eval_iter=step, + val_loss=val_result.cross_entropy_loss, + val_error=val_result.error_rate, + learning_rate=None) else: - self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=None, val_eval_iter=step, learning_rate=None) + self.train_stats_logger.update(train_eval_iter=None, + train_loss=None, + train_error=None, + total_train_error=None, + val_eval_iter=step, + learning_rate=None) # Save everything! self.train_stats_logger.log() # Save the model. if step % self.save_frequency == 0 and step > 0: - self.saver.save(self.sess, os.path.join(self.model_dir, GQCNNFilenames.INTER_MODEL.format(step))) - self.saver.save(self.sess, os.path.join(self.model_dir, GQCNNFilenames.FINAL_MODEL)) + self.saver.save( + self.sess, + os.path.join(self.model_dir, + GQCNNFilenames.INTER_MODEL.format(step))) + self.saver.save( + self.sess, + os.path.join(self.model_dir, + GQCNNFilenames.FINAL_MODEL)) # Launch tensorboard only after the first iteration. if not self.tensorboard_has_launched: @@ -445,39 +579,58 @@ def handler(signum, frame): # Get final errors and flush the stdout pipeline. final_val_result = self._error_rate_in_batches() - self.logger.info("Final validation error: {}".format(round(final_val_result.error_rate, 3))) - self.logger.info("Final validation loss: {}".format(round(final_val_result.cross_entropy_loss, 3))) + self.logger.info("Final validation error: {}".format( + round(final_val_result.error_rate, 3))) + self.logger.info("Final validation loss: {}".format( + round(final_val_result.cross_entropy_loss, 3))) if self.cfg["eval_total_train_error"]: - final_train_result = self._error_rate_in_batches(validation_set=False) - self.logger.info("Final training error: {}".format(final_train_result.error_rate)) - self.logger.info("Final training loss: {}".format(final_train_result.cross_entropy_loss)) + final_train_result = self._error_rate_in_batches( + validation_set=False) + self.logger.info("Final training error: {}".format( + final_train_result.error_rate)) + self.logger.info("Final training loss: {}".format( + final_train_result.cross_entropy_loss)) sys.stdout.flush() # Update the `TrainStatsLogger`. - self.train_stats_logger.update(train_eval_iter=None, train_loss=None, train_error=None, total_train_error=None, val_eval_iter=step, val_loss=final_val_result.cross_entropy_loss, val_error=final_val_result.error_rate, learning_rate=None) + self.train_stats_logger.update( + train_eval_iter=None, + train_loss=None, + train_error=None, + total_train_error=None, + val_eval_iter=step, + val_loss=final_val_result.cross_entropy_loss, + val_error=final_val_result.error_rate, + learning_rate=None) # Log & save everything! self.train_stats_logger.log() - self.saver.save(self.sess, os.path.join(self.model_dir, GQCNNFilenames.FINAL_MODEL)) + self.saver.save( + self.sess, + os.path.join(self.model_dir, GQCNNFilenames.FINAL_MODEL)) except Exception as e: self._cleanup() - raise + raise e self._cleanup() def _compute_data_metrics(self): - """Calculate image mean, image std, pose mean, pose std, normalization params.""" + """Calculate image mean, image std, pose mean, pose std, normalization + params.""" # Subsample tensors for faster runtime. random_file_indices = np.random.choice(self.num_tensors, size=self.num_random_files, replace=False) - + if self.gqcnn.input_depth_mode == InputDepthMode.POSE_STREAM: # Compute image stats. - im_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_MEAN) - im_std_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_STD) - if os.path.exists(im_mean_filename) and os.path.exists(im_std_filename): + im_mean_filename = os.path.join(self.model_dir, + GQCNNFilenames.IM_MEAN) + im_std_filename = os.path.join(self.model_dir, + GQCNNFilenames.IM_STD) + if os.path.exists(im_mean_filename) and os.path.exists( + im_std_filename): self.im_mean = np.load(im_mean_filename) self.im_std = np.load(im_std_filename) else: @@ -489,23 +642,29 @@ def _compute_data_metrics(self): num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to image mean estimate".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {} to image mean estimate". + format(k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: self.im_mean += np.sum(im_data[train_indices, ...]) - num_summed += self.train_index_map[i].shape[0] * im_data.shape[1] * im_data.shape[2] + num_summed += self.train_index_map[i].shape[ + 0] * im_data.shape[1] * im_data.shape[2] self.im_mean = self.im_mean / num_summed # Compute std. self.logger.info("Computing image std") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to image std estimate".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {} to image std estimate". + format(k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: - self.im_std += np.sum((im_data[train_indices, ...] - self.im_mean)**2) + self.im_std += np.sum( + (im_data[train_indices, ...] - self.im_mean)**2) self.im_std = np.sqrt(self.im_std / num_summed) # Save. @@ -517,9 +676,12 @@ def _compute_data_metrics(self): self.gqcnn.set_im_std(self.im_std) # Compute pose stats. - pose_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.POSE_MEAN) - pose_std_filename = os.path.join(self.model_dir, GQCNNFilenames.POSE_STD) - if os.path.exists(pose_mean_filename) and os.path.exists(pose_std_filename): + pose_mean_filename = os.path.join(self.model_dir, + GQCNNFilenames.POSE_MEAN) + pose_std_filename = os.path.join(self.model_dir, + GQCNNFilenames.POSE_STD) + if os.path.exists(pose_mean_filename) and os.path.exists( + pose_std_filename): self.pose_mean = np.load(pose_mean_filename) self.pose_std = np.load(pose_std_filename) else: @@ -531,22 +693,29 @@ def _compute_data_metrics(self): self.logger.info("Computing pose mean") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to pose mean estimate".format(k+1, random_file_indices.shape[0])) - pose_data = self.dataset.tensor(self.pose_field_name, i).arr + self.logger.info( + "Adding file {} of {} to pose mean estimate". + format(k + 1, random_file_indices.shape[0])) + pose_data = self.dataset.tensor(self.pose_field_name, + i).arr train_indices = self.train_index_map[i] if self.gripper_mode == GripperMode.SUCTION: - rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]//2, - replace=False) - pose_data[rand_indices, 4] = -pose_data[rand_indices, 4] + rand_indices = np.random.choice( + pose_data.shape[0], + size=pose_data.shape[0] // 2, + replace=False) + pose_data[rand_indices, + 4] = -pose_data[rand_indices, 4] elif self.gripper_mode == GripperMode.LEGACY_SUCTION: - rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]//2, - replace=False) - pose_data[rand_indices, 3] = -pose_data[rand_indices, 3] + rand_indices = np.random.choice( + pose_data.shape[0], + size=pose_data.shape[0] // 2, + replace=False) + pose_data[rand_indices, + 3] = -pose_data[rand_indices, 3] if train_indices.shape[0] > 0: - pose_data = pose_data[train_indices,:] - pose_data = pose_data[np.isfinite(pose_data[:,3]),:] + pose_data = pose_data[train_indices, :] + pose_data = pose_data[np.isfinite(pose_data[:, 3]), :] self.pose_mean += np.sum(pose_data, axis=0) num_summed += pose_data.shape[0] self.pose_mean = self.pose_mean / num_summed @@ -555,29 +724,39 @@ def _compute_data_metrics(self): self.logger.info("Computing pose std") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to pose std estimate".format(k+1, random_file_indices.shape[0])) - pose_data = self.dataset.tensor(self.pose_field_name, i).arr + self.logger.info( + "Adding file {} of {} to pose std estimate".format( + k + 1, random_file_indices.shape[0])) + pose_data = self.dataset.tensor(self.pose_field_name, + i).arr train_indices = self.train_index_map[i] if self.gripper_mode == GripperMode.SUCTION: - rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]//2, - replace=False) - pose_data[rand_indices, 4] = -pose_data[rand_indices, 4] + rand_indices = np.random.choice( + pose_data.shape[0], + size=pose_data.shape[0] // 2, + replace=False) + pose_data[rand_indices, + 4] = -pose_data[rand_indices, 4] elif self.gripper_mode == GripperMode.LEGACY_SUCTION: - rand_indices = np.random.choice(pose_data.shape[0], - size=pose_data.shape[0]//2, - replace=False) - pose_data[rand_indices, 3] = -pose_data[rand_indices, 3] + rand_indices = np.random.choice( + pose_data.shape[0], + size=pose_data.shape[0] // 2, + replace=False) + pose_data[rand_indices, + 3] = -pose_data[rand_indices, 3] if train_indices.shape[0] > 0: - pose_data = pose_data[train_indices,:] - pose_data = pose_data[np.isfinite(pose_data[:,3]),:] - self.pose_std += np.sum((pose_data - self.pose_mean)**2, axis=0) + pose_data = pose_data[train_indices, :] + pose_data = pose_data[np.isfinite(pose_data[:, 3]), :] + self.pose_std += np.sum( + (pose_data - self.pose_mean)**2, axis=0) self.pose_std = np.sqrt(self.pose_std / num_summed) - self.pose_std[self.pose_std==0] = 1.0 + self.pose_std[self.pose_std == 0] = 1.0 # Save. - self.pose_mean = read_pose_data(self.pose_mean, self.gripper_mode) - self.pose_std = read_pose_data(self.pose_std, self.gripper_mode) + self.pose_mean = read_pose_data(self.pose_mean, + self.gripper_mode) + self.pose_std = read_pose_data(self.pose_std, + self.gripper_mode) np.save(pose_mean_filename, self.pose_mean) np.save(pose_std_filename, self.pose_std) @@ -586,15 +765,20 @@ def _compute_data_metrics(self): self.gqcnn.set_pose_std(self.pose_std) # Check for invalid values. - if np.any(np.isnan(self.pose_mean)) or np.any(np.isnan(self.pose_std)): - self.logger.error("Pose mean or pose std is NaN! Check the input dataset") + if np.any(np.isnan(self.pose_mean)) or np.any( + np.isnan(self.pose_std)): + self.logger.error( + "Pose mean or pose std is NaN! Check the input dataset") exit(0) elif self.gqcnn.input_depth_mode == InputDepthMode.SUB: # Compute (image - depth) stats. - im_depth_sub_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_DEPTH_SUB_MEAN) - im_depth_sub_std_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_DEPTH_SUB_STD) - if os.path.exists(im_depth_sub_mean_filename) and os.path.exists(im_depth_sub_std_filename): + im_depth_sub_mean_filename = os.path.join( + self.model_dir, GQCNNFilenames.IM_DEPTH_SUB_MEAN) + im_depth_sub_std_filename = os.path.join( + self.model_dir, GQCNNFilenames.IM_DEPTH_SUB_STD) + if os.path.exists(im_depth_sub_mean_filename) and os.path.exists( + im_depth_sub_std_filename): self.im_depth_sub_mean = np.load(im_depth_sub_mean_filename) self.im_depth_sub_std = np.load(im_depth_sub_std_filename) else: @@ -602,32 +786,49 @@ def _compute_data_metrics(self): self.im_depth_sub_std = 0 # Compute mean. - self.logger.info("Computing (image - depth) mean") + self.logger.info("Computing (image - depth) mean.") num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to (image - depth) mean estimate".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {}..." + .format(k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr - depth_data = read_pose_data(self.dataset.tensor(self.pose_field_name, i).arr, self.gripper_mode) - sub_data = im_data - np.tile(np.reshape(depth_data, (-1, 1, 1, 1)), (1, im_data.shape[1], im_data.shape[2], 1)) + depth_data = read_pose_data( + self.dataset.tensor(self.pose_field_name, i).arr, + self.gripper_mode) + sub_data = im_data - np.tile( + np.reshape(depth_data, (-1, 1, 1, 1)), + (1, im_data.shape[1], im_data.shape[2], 1)) train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: - self.im_depth_sub_mean += np.sum(sub_data[train_indices, ...]) - num_summed += self.train_index_map[i].shape[0] * im_data.shape[1] * im_data.shape[2] + self.im_depth_sub_mean += np.sum( + sub_data[train_indices, ...]) + num_summed += self.train_index_map[i].shape[ + 0] * im_data.shape[1] * im_data.shape[2] self.im_depth_sub_mean = self.im_depth_sub_mean / num_summed # Compute std. - self.logger.info("Computing (image - depth) std") + self.logger.info("Computing (image - depth) std.") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to (image - depth) std estimate".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {}..." + .format(k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr - depth_data = read_pose_data(self.dataset.tensor(self.pose_field_name, i).arr, self.gripper_mode) - sub_data = im_data - np.tile(np.reshape(depth_data, (-1, 1, 1, 1)), (1, im_data.shape[1], im_data.shape[2], 1)) + depth_data = read_pose_data( + self.dataset.tensor(self.pose_field_name, i).arr, + self.gripper_mode) + sub_data = im_data - np.tile( + np.reshape(depth_data, (-1, 1, 1, 1)), + (1, im_data.shape[1], im_data.shape[2], 1)) train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: - self.im_depth_sub_std += np.sum((sub_data[train_indices, ...] - self.im_depth_sub_mean)**2) - self.im_depth_sub_std = np.sqrt(self.im_depth_sub_std / num_summed) + self.im_depth_sub_std += np.sum( + (sub_data[train_indices, ...] - + self.im_depth_sub_mean)**2) + self.im_depth_sub_std = np.sqrt(self.im_depth_sub_std / + num_summed) # Save. np.save(im_depth_sub_mean_filename, self.im_depth_sub_mean) @@ -639,9 +840,12 @@ def _compute_data_metrics(self): elif self.gqcnn.input_depth_mode == InputDepthMode.IM_ONLY: # Compute image stats. - im_mean_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_MEAN) - im_std_filename = os.path.join(self.model_dir, GQCNNFilenames.IM_STD) - if os.path.exists(im_mean_filename) and os.path.exists(im_std_filename): + im_mean_filename = os.path.join(self.model_dir, + GQCNNFilenames.IM_MEAN) + im_std_filename = os.path.join(self.model_dir, + GQCNNFilenames.IM_STD) + if os.path.exists(im_mean_filename) and os.path.exists( + im_std_filename): self.im_mean = np.load(im_mean_filename) self.im_std = np.load(im_std_filename) else: @@ -649,27 +853,33 @@ def _compute_data_metrics(self): self.im_std = 0 # Compute mean. - self.logger.info("Computing image mean") + self.logger.info("Computing image mean.") num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to image mean estimate".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {}..." + .format(k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: self.im_mean += np.sum(im_data[train_indices, ...]) - num_summed += self.train_index_map[i].shape[0] * im_data.shape[1] * im_data.shape[2] + num_summed += self.train_index_map[i].shape[ + 0] * im_data.shape[1] * im_data.shape[2] self.im_mean = self.im_mean / num_summed # Compute std. - self.logger.info("Computing image std") + self.logger.info("Computing image std.") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to image std estimate".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {}..." + .format(k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: - self.im_std += np.sum((im_data[train_indices, ...] - self.im_mean)**2) + self.im_std += np.sum( + (im_data[train_indices, ...] - self.im_mean)**2) self.im_std = np.sqrt(self.im_std / num_summed) # Save. @@ -679,22 +889,27 @@ def _compute_data_metrics(self): # Update GQ-CNN instance. self.gqcnn.set_im_mean(self.im_mean) self.gqcnn.set_im_std(self.im_std) - + # Compute normalization parameters of the network. - pct_pos_train_filename = os.path.join(self.model_dir, GQCNNFilenames.PCT_POS_TRAIN) - pct_pos_val_filename = os.path.join(self.model_dir, GQCNNFilenames.PCT_POS_VAL) - if os.path.exists(pct_pos_train_filename) and os.path.exists(pct_pos_val_filename): + pct_pos_train_filename = os.path.join(self.model_dir, + GQCNNFilenames.PCT_POS_TRAIN) + pct_pos_val_filename = os.path.join(self.model_dir, + GQCNNFilenames.PCT_POS_VAL) + if os.path.exists(pct_pos_train_filename) and os.path.exists( + pct_pos_val_filename): pct_pos_train = np.load(pct_pos_train_filename) pct_pos_val = np.load(pct_pos_val_filename) else: - self.logger.info("Computing grasp quality metric stats") + self.logger.info("Computing grasp quality metric stats.") all_train_metrics = None all_val_metrics = None - + # Read metrics. for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info("Adding file {} of {} to metric stat estimates".format(k+1, random_file_indices.shape[0])) + self.logger.info( + "Adding file {} of {}...".format( + k + 1, random_file_indices.shape[0])) metric_data = self.dataset.tensor(self.label_field_name, i).arr train_indices = self.train_index_map[i] val_indices = self.val_index_map[i] @@ -704,14 +919,16 @@ def _compute_data_metrics(self): if all_train_metrics is None: all_train_metrics = train_metric_data else: - all_train_metrics = np.r_[all_train_metrics, train_metric_data] + all_train_metrics = np.r_[all_train_metrics, + train_metric_data] if val_indices.shape[0] > 0: val_metric_data = metric_data[val_indices] if all_val_metrics is None: all_val_metrics = val_metric_data else: - all_val_metrics = np.r_[all_val_metrics, val_metric_data] + all_val_metrics = np.r_[all_val_metrics, + val_metric_data] # Compute train stats. self.min_metric = np.min(all_train_metrics) @@ -720,20 +937,22 @@ def _compute_data_metrics(self): self.median_metric = np.median(all_train_metrics) # Save metrics. - pct_pos_train = np.sum(all_train_metrics > self.metric_thresh) / all_train_metrics.shape[0] + pct_pos_train = np.sum(all_train_metrics > self.metric_thresh + ) / all_train_metrics.shape[0] np.save(pct_pos_train_filename, np.array(pct_pos_train)) if self.train_pct < 1.0: - pct_pos_val = np.sum(all_val_metrics > self.metric_thresh) / all_val_metrics.shape[0] + pct_pos_val = np.sum(all_val_metrics > self.metric_thresh + ) / all_val_metrics.shape[0] np.save(pct_pos_val_filename, np.array(pct_pos_val)) - + self.logger.info("Percent positive in train: " + str(pct_pos_train)) if self.train_pct < 1.0: self.logger.info("Percent positive in val: " + str(pct_pos_val)) if self._angular_bins > 0: self.logger.info("Calculating angular bin statistics...") - bin_counts = np.zeros((self._angular_bins,)) + bin_counts = np.zeros((self._angular_bins, )) for m in range(self.num_tensors): pose_arr = self.dataset.tensor(self.pose_field_name, m).arr angles = pose_arr[:, 3] @@ -744,15 +963,16 @@ def _compute_data_metrics(self): l_neg_90 = np.where(angles < (-1 * (self._max_angle / 2))) angles[g_90] -= self._max_angle angles[l_neg_90] += self._max_angle - angles *= -1 # Hack to fix reverse angle convention. - # TODO(vsatish): Actually fix this. + # TODO(vsatish): Actually fix this. + angles *= -1 # Hack to fix reverse angle convention. angles += (self._max_angle / 2) for i in range(angles.shape[0]): bin_counts[int(angles[i] // self._bin_width)] += 1 - self.logger.info("Bin counts: {}".format(bin_counts)) + self.logger.info("Bin counts: {}.".format(bin_counts)) def _compute_split_indices(self): - """ Compute train and validation indices for each tensor to speed data accesses """ + """Compute train and validation indices for each tensor to speed data + accesses.""" # Read indices. train_indices, val_indices, _ = self.dataset.split(self.split_name) @@ -760,42 +980,46 @@ def _compute_split_indices(self): self.train_index_map = {} for i in range(self.dataset.num_tensors): self.train_index_map[i] = [] - + for i in train_indices: tensor_index = self.dataset.tensor_index(i) - datapoint_indices = self.dataset.datapoint_indices_for_tensor(tensor_index) + datapoint_indices = self.dataset.datapoint_indices_for_tensor( + tensor_index) lowest = np.min(datapoint_indices) self.train_index_map[tensor_index].append(i - lowest) for i, indices in self.train_index_map.items(): self.train_index_map[i] = np.array(indices) - + self.val_index_map = {} for i in range(self.dataset.num_tensors): self.val_index_map[i] = [] - + for i in val_indices: tensor_index = self.dataset.tensor_index(i) if tensor_index not in self.val_index_map: self.val_index_map[tensor_index] = [] - datapoint_indices = self.dataset.datapoint_indices_for_tensor(tensor_index) + datapoint_indices = self.dataset.datapoint_indices_for_tensor( + tensor_index) lowest = np.min(datapoint_indices) self.val_index_map[tensor_index].append(i - lowest) for i, indices in self.val_index_map.items(): self.val_index_map[i] = np.array(indices) - + def _setup_output_dirs(self): """Setup output directories.""" self.logger.info("Saving model to: {}".format(self.model_dir)) # Create the summary dir. - self.summary_dir = os.path.join(self.model_dir, "tensorboard_summaries") + self.summary_dir = os.path.join(self.model_dir, + "tensorboard_summaries") if not os.path.exists(self.summary_dir): os.mkdir(self.summary_dir) else: - # If the summary directory already exists, clean it out by deleting all files in it - # we don"t want tensorboard to get confused with old logs while debugging with the same directory. + # If the summary directory already exists, clean it out by deleting + # all files in it. We don't want Tensorboard to get confused with + # old logs while reusing the same directory. old_files = os.listdir(self.summary_dir) for f in old_files: os.remove(os.path.join(self.summary_dir, f)) @@ -804,34 +1028,36 @@ def _setup_output_dirs(self): self.filter_dir = os.path.join(self.model_dir, "filters") if not os.path.exists(self.filter_dir): os.mkdir(self.filter_dir) - + def _save_configs(self): """Save training configuration.""" # Update config for fine-tuning. if self.finetuning: - self.cfg["base_model_dir"] = self.base_model_dir + self.cfg["base_model_dir"] = self.base_model_dir # Save config. - out_config_filename = os.path.join(self.model_dir, GQCNNFilenames.SAVED_CFG) + out_config_filename = os.path.join(self.model_dir, + GQCNNFilenames.SAVED_CFG) tempOrderedDict = collections.OrderedDict() for key in self.cfg: tempOrderedDict[key] = self.cfg[key] with open(out_config_filename, "w") as outfile: json.dump(tempOrderedDict, outfile, - indent=GeneralConstants.JSON_INDENT) + indent=JSON_INDENT) - # Save training script. + # Save training script. this_filename = sys.argv[0] out_train_filename = os.path.join(self.model_dir, "training_script.py") shutil.copyfile(this_filename, out_train_filename) # Save architecture. - out_architecture_filename = os.path.join(self.model_dir, GQCNNFilenames.SAVED_ARCH) + out_architecture_filename = os.path.join(self.model_dir, + GQCNNFilenames.SAVED_ARCH) json.dump(self.cfg["gqcnn"]["architecture"], open(out_architecture_filename, "w"), - indent=GeneralConstants.JSON_INDENT) - + indent=JSON_INDENT) + def _read_training_params(self): """Read training parameters from configuration file.""" # Splits. @@ -844,7 +1070,7 @@ def _read_training_params(self): self.max_files_eval = None if "max_files_eval" in self.cfg: self.max_files_eval = self.cfg["max_files_eval"] - + # Logging. self.num_epochs = self.cfg["num_epochs"] self.eval_frequency = self.cfg["eval_frequency"] @@ -857,23 +1083,26 @@ def _read_training_params(self): self.decay_step_multiplier = self.cfg["decay_step_multiplier"] self.decay_rate = self.cfg["decay_rate"] self.momentum_rate = self.cfg["momentum_rate"] - self.max_training_examples_per_load = self.cfg["max_training_examples_per_load"] + self.max_training_examples_per_load = self.cfg[ + "max_training_examples_per_load"] self.drop_rate = self.cfg["drop_rate"] self.max_global_grad_norm = self.cfg["max_global_grad_norm"] self.optimize_base_layers = False if "optimize_base_layers" in self.cfg: self.optimize_base_layers = self.cfg["optimize_base_layers"] - + # Metrics. self.target_metric_name = self.cfg["target_metric_name"] self.metric_thresh = self.cfg["metric_thresh"] self.training_mode = self.cfg["training_mode"] if self.training_mode != TrainingMode.CLASSIFICATION: - raise ValueError("Training mode \"{}\" not currently supported!".format(self.training_mode)) - + raise ValueError( + "Training mode \"{}\" not currently supported!".format( + self.training_mode)) + # Tensorboad. self._tensorboard_port = self.cfg["tensorboard_port"] - + # Preprocessing. self.preproc_log_frequency = self.cfg["preproc_log_frequency"] self.num_random_files = self.cfg["num_random_files"] @@ -894,7 +1123,7 @@ def _read_training_params(self): self.neg_accept_prob = 1 / self.pos_weight else: self.pos_accept_prob = self.pos_weight - + if self.train_pct < 0 or self.train_pct > 1: raise ValueError("Train percentage must be in range [0,1]") @@ -904,8 +1133,8 @@ def _read_training_params(self): # Input normalization. self._norm_inputs = True if self.gqcnn.input_depth_mode == InputDepthMode.SUB: - self._norm_inputs = False - + self._norm_inputs = False + # Angular training. self._angular_bins = self.gqcnn.angular_bins self._max_angle = self.gqcnn.max_angle @@ -913,7 +1142,9 @@ def _read_training_params(self): # During angular training, make sure symmetrization in denoising is # turned off and also set the angular bin width. if self._angular_bins > 0: - assert not self.cfg["symmetrize"], "Symmetrization denoising must be turned off during angular training" + symmetrization_msg = ("Symmetrization denoising must be turned off" + " during angular training.") + assert not self.cfg["symmetrize"], symmetrization_msg self._bin_width = self._max_angle / self._angular_bins # Debugging. @@ -921,9 +1152,11 @@ def _read_training_params(self): self._seed = self.cfg["seed"] if self._debug: if self.num_prefetch_q_workers > 1: - self.logger.warning("Deterministic execution is not possible with " - "more than one prefetch queue worker even in debug mode.") - self.num_random_files = self.cfg["debug_num_files"] # This reduces initialization time for fast debugging. + self.logger.warning( + "Deterministic execution is not possible with " + "more than one prefetch queue worker even in debug mode.") + # This reduces initialization time for fast debugging. + self.num_random_files = self.cfg["debug_num_files"] np.random.seed(self._seed) random.seed(self._seed) @@ -935,10 +1168,12 @@ def _setup_denoising_and_synthetic(self): self.gamma_shape = self.cfg["gamma_shape"] self.gamma_scale = 1 / self.gamma_shape - # Gaussian process noise. + # Gaussian process noise. if self.cfg["gaussian_process_denoising"]: - self.gp_rescale_factor = self.cfg["gaussian_process_scaling_factor"] - self.gp_sample_height = int(self.im_height / self.gp_rescale_factor) + self.gp_rescale_factor = self.cfg[ + "gaussian_process_scaling_factor"] + self.gp_sample_height = int(self.im_height / + self.gp_rescale_factor) self.gp_sample_width = int(self.im_width / self.gp_rescale_factor) self.gp_num_pix = self.gp_sample_height * self.gp_sample_width self.gp_sigma = self.cfg["gaussian_process_sigma"] @@ -954,27 +1189,35 @@ def _open_dataset(self): # Read split. if not self.dataset.has_split(self.split_name): - self.logger.info("Training split: {} not found in dataset. Creating new split...".format(self.split_name)) + self.logger.info( + "Dataset split: {} not found. Creating new split..." + .format(self.split_name)) self.dataset.make_split(self.split_name, train_pct=self.train_pct) else: - self.logger.info("Training split: {} found in dataset.".format(self.split_name)) + self.logger.info("Training split: {} found in dataset.".format( + self.split_name)) self._compute_split_indices() - + def _compute_data_params(self): """Compute parameters of the dataset.""" # Image params. self.im_field_name = self.cfg["image_field_name"] - self.im_height = self.dataset.config["fields"][self.im_field_name]["height"] - self.im_width = self.dataset.config["fields"][self.im_field_name]["width"] - self.im_channels = self.dataset.config["fields"][self.im_field_name]["channels"] - self.im_center = np.array([self.im_height // 2, self.im_width // 2]) # NOTE: There was originally some weird math going on here... + self.im_height = self.dataset.config["fields"][ + self.im_field_name]["height"] + self.im_width = self.dataset.config["fields"][ + self.im_field_name]["width"] + self.im_channels = self.dataset.config["fields"][ + self.im_field_name]["channels"] + # NOTE: There was originally some weird math going on here... + self.im_center = np.array([self.im_height // 2, self.im_width // 2]) # Poses. self.pose_field_name = self.cfg["pose_field_name"] self.gripper_mode = self.gqcnn.gripper_mode self.pose_dim = pose_dim(self.gripper_mode) - self.raw_pose_shape = self.dataset.config["fields"][self.pose_field_name]["height"] - + self.raw_pose_shape = self.dataset.config["fields"][ + self.pose_field_name]["height"] + # Outputs. self.label_field_name = self.target_metric_name self.num_categories = 2 @@ -987,9 +1230,14 @@ def _compute_data_params(self): for val_indices in self.train_index_map.values(): self.num_val += val_indices.shape[0] - # Set params based on the number of training examples (convert epochs to steps). - self.eval_frequency = int(np.ceil(self.eval_frequency * (self.num_train / self.train_batch_size))) - self.save_frequency = int(np.ceil(self.save_frequency * (self.num_train / self.train_batch_size))) + # Set params based on the number of training examples (convert epochs + # to steps). + self.eval_frequency = int( + np.ceil(self.eval_frequency * + (self.num_train / self.train_batch_size))) + self.save_frequency = int( + np.ceil(self.save_frequency * + (self.num_train / self.train_batch_size))) self.decay_step = self.decay_step_multiplier * self.num_train def _setup_tensorflow(self): @@ -1004,24 +1252,31 @@ def _setup_tensorflow(self): self.numpy_dtype = np.int64 if self.cfg["loss"] == "weighted_cross_entropy": train_label_dtype = tf.float32 - self.numpy_dtype = np.float32 + self.numpy_dtype = np.float32 else: - raise ValueError("Training mode \"{}\" not supported".format(self.training_mode)) + raise ValueError("Training mode \"{}\" not supported".format( + self.training_mode)) # Set up placeholders. - self.train_labels_node = tf.placeholder(train_label_dtype, (self.train_batch_size,)) - self.input_im_node = tf.placeholder(tf.float32, (self.train_batch_size, self.im_height, self.im_width, self.im_channels)) - self.input_pose_node = tf.placeholder(tf.float32, (self.train_batch_size, self.pose_dim)) + self.train_labels_node = tf.placeholder(train_label_dtype, + (self.train_batch_size, )) + self.input_im_node = tf.placeholder( + tf.float32, (self.train_batch_size, self.im_height, self.im_width, + self.im_channels)) + self.input_pose_node = tf.placeholder( + tf.float32, (self.train_batch_size, self.pose_dim)) if self._angular_bins > 0: - self.train_pred_mask_node = tf.placeholder(tf.int32, (self.train_batch_size, self._angular_bins * 2)) - + self.train_pred_mask_node = tf.placeholder( + tf.int32, (self.train_batch_size, self._angular_bins * 2)) + # Create data prefetch queue. self.prefetch_q = mp.Queue(self.max_prefetch_q_size) # Get weights. self.weights = self.gqcnn.weights - - # Open a TF session for the GQ-CNN instance and store it also as the optimizer session. + + # Open a TF session for the GQ-CNN instance and store it also as the + # optimizer session. self.sess = self.gqcnn.open_session() # Setup data prefetch queue worker termination event. @@ -1029,9 +1284,10 @@ def _setup_tensorflow(self): self.term_event.clear() def _setup_summaries(self): - """Sets up placeholders for summary values and creates summary writer.""" - # Create placeholders for Python values because `tf.summary.scalar` expects - # a placeholder. + """Sets up placeholders for summary values and creates summary + writer.""" + # Create placeholders for Python values because `tf.summary.scalar` + # expects a placeholder. self.val_error_placeholder = tf.placeholder(tf.float32, []) self.minibatch_error_placeholder = tf.placeholder(tf.float32, []) self.minibatch_loss_placeholder = tf.placeholder(tf.float32, []) @@ -1039,10 +1295,18 @@ def _setup_summaries(self): # Tag the `tf.summary.scalar`s so that we can group them together and # write different batches of summaries at different intervals. - tf.summary.scalar("val_error", self.val_error_placeholder, collections=["eval_frequency"]) - tf.summary.scalar("minibatch_error", self.minibatch_error_placeholder, collections=["log_frequency"]) - tf.summary.scalar("minibatch_loss", self.minibatch_loss_placeholder, collections=["log_frequency"]) - tf.summary.scalar("learning_rate", self.learning_rate_placeholder, collections=["log_frequency"]) + tf.summary.scalar("val_error", + self.val_error_placeholder, + collections=["eval_frequency"]) + tf.summary.scalar("minibatch_error", + self.minibatch_error_placeholder, + collections=["log_frequency"]) + tf.summary.scalar("minibatch_loss", + self.minibatch_loss_placeholder, + collections=["log_frequency"]) + tf.summary.scalar("learning_rate", + self.learning_rate_placeholder, + collections=["log_frequency"]) self.merged_eval_summaries = tf.summary.merge_all("eval_frequency") self.merged_log_summaries = tf.summary.merge_all("log_frequency") @@ -1052,10 +1316,10 @@ def _setup_summaries(self): # Initialize the variables again now that we have added some new ones. with self.sess.as_default(): tf.global_variables_initializer().run() - + def _cleanup(self): self.logger.info("Cleaning and preparing to exit optimization...") - + # Set termination event for prefetch queue workers. self.logger.info("Terminating prefetch queue workers...") self.term_event.set() @@ -1074,13 +1338,13 @@ def _cleanup(self): # Close Tensorflow session. self.gqcnn.close_session() - + def _flush_prefetch_queue(self): """Flush prefetch queue.""" self.logger.info("Flushing prefetch queue...") for i in range(self.prefetch_q.qsize()): self.prefetch_q.get() - + def _setup(self): """Setup for training.""" # Initialize data prefetch queue thread exit booleans. @@ -1097,14 +1361,14 @@ def _setup(self): self._read_training_params() # Setup image and pose data files. - self._open_dataset() + self._open_dataset() # Compute data parameters. self._compute_data_params() - + # Setup denoising and synthetic data parameters. self._setup_denoising_and_synthetic() - + # Compute means, std's, and normalization metrics. self._compute_data_metrics() @@ -1116,10 +1380,13 @@ def _setup(self): def _load_and_enqueue(self, seed): """Loads and enqueues a batch of images for training.""" - signal.signal(signal.SIGINT, signal.SIG_IGN) # When the parent process receives a SIGINT, it will itself handle cleaning up child processes. - # Set the random seed explicitly to prevent all workers from possible inheriting - # the same seed on process initialization. + # When the parent process receives a SIGINT, it will itself handle + # cleaning up child processes. + signal.signal(signal.SIGINT, signal.SIG_IGN) + + # Set the random seed explicitly to prevent all workers from possible + # inheriting the same seed on process initialization. np.random.seed(seed) random.seed(seed) @@ -1135,55 +1402,70 @@ def _load_and_enqueue(self, seed): queue_start = time.time() # Init buffers. - train_images = np.zeros( - [self.train_batch_size, self.im_height, self.im_width, self.im_channels]).astype(np.float32) - train_poses = np.zeros([self.train_batch_size, self.pose_dim]).astype(np.float32) - train_labels = np.zeros(self.train_batch_size).astype(self.numpy_dtype) + train_images = np.zeros([ + self.train_batch_size, self.im_height, self.im_width, + self.im_channels + ]).astype(np.float32) + train_poses = np.zeros([self.train_batch_size, + self.pose_dim]).astype(np.float32) + train_labels = np.zeros(self.train_batch_size).astype( + self.numpy_dtype) if self._angular_bins > 0: - train_pred_mask = np.zeros((self.train_batch_size, self._angular_bins*2), dtype=bool) - + train_pred_mask = np.zeros( + (self.train_batch_size, self._angular_bins * 2), + dtype=bool) + while start_i < self.train_batch_size: # Compute num remaining. num_remaining = self.train_batch_size - num_queued - + # Generate tensor index uniformly at random. file_num = np.random.choice(self.num_tensors, size=1)[0] read_start = time.time() - train_images_tensor = dataset.tensor(self.im_field_name, file_num) - train_poses_tensor = dataset.tensor(self.pose_field_name, file_num) - train_labels_tensor = dataset.tensor(self.label_field_name, file_num) + train_images_tensor = dataset.tensor(self.im_field_name, + file_num) + train_poses_tensor = dataset.tensor(self.pose_field_name, + file_num) + train_labels_tensor = dataset.tensor(self.label_field_name, + file_num) read_stop = time.time() - self.logger.debug("Reading data took {} sec".format(round(read_stop - read_start, 3))) + self.logger.debug("Reading data took {} sec".format( + round(read_stop - read_start, 3))) self.logger.debug("File num: {}".format(file_num)) - + # Get batch indices uniformly at random. train_ind = self.train_index_map[file_num] np.random.shuffle(train_ind) if self.gripper_mode == GripperMode.LEGACY_SUCTION: - tp_tmp = read_pose_data(train_poses_tensor.data, self.gripper_mode) - train_ind = train_ind[np.isfinite(tp_tmp[train_ind,1])] - + tp_tmp = read_pose_data(train_poses_tensor.data, + self.gripper_mode) + train_ind = train_ind[np.isfinite(tp_tmp[train_ind, 1])] + # Filter positives and negatives. - if self.training_mode == TrainingMode.CLASSIFICATION and self.pos_weight != 0.0: + if self.training_mode == TrainingMode.CLASSIFICATION and \ + self.pos_weight != 0.0: labels = 1 * (train_labels_tensor.arr > self.metric_thresh) np.random.shuffle(train_ind) filtered_ind = [] for index in train_ind: - if labels[index] == 0 and np.random.rand() < self.neg_accept_prob: + if labels[index] == 0 and np.random.rand( + ) < self.neg_accept_prob: filtered_ind.append(index) - elif labels[index] == 1 and np.random.rand() < self.pos_accept_prob: + elif labels[index] == 1 and np.random.rand( + ) < self.pos_accept_prob: filtered_ind.append(index) train_ind = np.array(filtered_ind) # Samples train indices. - upper = min(num_remaining, train_ind.shape[0], self.max_training_examples_per_load) + upper = min(num_remaining, train_ind.shape[0], + self.max_training_examples_per_load) ind = train_ind[:upper] num_loaded = ind.shape[0] if num_loaded == 0: self.logger.warning("Queueing zero examples!!!!") continue - + # Subsample data. train_images_arr = train_images_tensor.arr[ind, ...] train_poses_arr = train_poses_tensor.arr[ind, ...] @@ -1194,19 +1476,22 @@ def _load_and_enqueue(self, seed): # Resize images. rescale_factor = self.im_height / train_images_arr.shape[1] if rescale_factor != 1.0: - resized_train_images_arr = np.zeros([num_images, - self.im_height, - self.im_width, - self.im_channels]).astype(np.float32) + resized_train_images_arr = np.zeros([ + num_images, self.im_height, self.im_width, + self.im_channels + ]).astype(np.float32) for i in range(num_images): for c in range(train_images_arr.shape[3]): - resized_train_images_arr[i,:,:,c] = sm.imresize(train_images_arr[i,:,:,c], - rescale_factor, - interp="bicubic", mode="F") + resized_train_images_arr[i, :, :, c] = sm.imresize( + train_images_arr[i, :, :, c], + rescale_factor, + interp="bicubic", + mode="F") train_images_arr = resized_train_images_arr - + # Add noises to images. - train_images_arr, train_poses_arr = self._distort(train_images_arr, train_poses_arr) + train_images_arr, train_poses_arr = self._distort( + train_images_arr, train_poses_arr) # Slice poses. train_poses_arr = read_pose_data(train_poses_arr, @@ -1214,9 +1499,12 @@ def _load_and_enqueue(self, seed): # Standardize inputs and outputs. if self._norm_inputs: - train_images_arr = (train_images_arr - self.im_mean) / self.im_std - if self.gqcnn.input_depth_mode == InputDepthMode.POSE_STREAM: - train_poses_arr = (train_poses_arr - self.pose_mean) / self.pose_std + train_images_arr = (train_images_arr - + self.im_mean) / self.im_std + if self.gqcnn.input_depth_mode == \ + InputDepthMode.POSE_STREAM: + train_poses_arr = (train_poses_arr - + self.pose_mean) / self.pose_std train_label_arr = 1 * (train_label_arr > self.metric_thresh) train_label_arr = train_label_arr.astype(self.numpy_dtype) @@ -1230,22 +1518,25 @@ def _load_and_enqueue(self, seed): l_neg_90 = np.where(angles < (-1 * (self._max_angle / 2))) angles[g_90] -= self._max_angle angles[l_neg_90] += self._max_angle - angles *= -1 # Hack to fix reverse angle convention. - # TODO(vsatish): Actually fix this. + # TODO(vsatish): Actually fix this. + angles *= -1 # Hack to fix reverse angle convention. angles += (self._max_angle / 2) - train_pred_mask_arr = np.zeros((train_label_arr.shape[0], self._angular_bins*2)) + train_pred_mask_arr = np.zeros( + (train_label_arr.shape[0], self._angular_bins * 2)) for i in range(angles.shape[0]): bins[i] = angles[i] // self._bin_width - train_pred_mask_arr[i, int((angles[i] // self._bin_width)*2)] = 1 - train_pred_mask_arr[i, int((angles[i] // self._bin_width)*2 + 1)] = 1 + train_pred_mask_arr[ + i, int((angles[i] // self._bin_width) * 2)] = 1 + train_pred_mask_arr[ + i, int((angles[i] // self._bin_width) * 2 + 1)] = 1 # Compute the number of examples loaded. num_loaded = train_images_arr.shape[0] end_i = start_i + num_loaded - + # Enqueue training data batch. train_images[start_i:end_i, ...] = train_images_arr.copy() - train_poses[start_i:end_i,:] = train_poses_arr.copy() + train_poses[start_i:end_i, :] = train_poses_arr.copy() train_labels[start_i:end_i] = train_label_arr.copy() if self._angular_bins > 0: train_pred_mask[start_i:end_i] = train_pred_mask_arr.copy() @@ -1258,49 +1549,67 @@ def _load_and_enqueue(self, seed): if not self.term_event.is_set(): try: if self._angular_bins > 0: - self.prefetch_q.put_nowait((train_images, train_poses, train_labels, train_pred_mask)) + self.prefetch_q.put_nowait( + (train_images, train_poses, train_labels, + train_pred_mask)) else: - self.prefetch_q.put_nowait((train_images, train_poses, train_labels)) + self.prefetch_q.put_nowait( + (train_images, train_poses, train_labels)) except Queue.Full: time.sleep(GeneralConstants.QUEUE_SLEEP) queue_stop = time.time() - self.logger.debug("Queue batch took {} sec".format(round(queue_stop - queue_start, 3))) + self.logger.debug("Queue batch took {} sec".format( + round(queue_stop - queue_start, 3))) def _distort(self, image_arr, pose_arr): """Adds noise to a batch of images.""" # Read params. num_images = image_arr.shape[0] - + # Denoising and synthetic data generation. if self.cfg["multiplicative_denoising"]: - mult_samples = ss.gamma.rvs(self.gamma_shape, scale=self.gamma_scale, size=num_images) - mult_samples = mult_samples[:,np.newaxis,np.newaxis,np.newaxis] - image_arr = image_arr * np.tile(mult_samples, [1, self.im_height, self.im_width, self.im_channels]) + mult_samples = ss.gamma.rvs(self.gamma_shape, + scale=self.gamma_scale, + size=num_images) + mult_samples = mult_samples[:, np.newaxis, np.newaxis, np.newaxis] + image_arr = image_arr * np.tile( + mult_samples, + [1, self.im_height, self.im_width, self.im_channels]) # Add correlated Gaussian noise. if self.cfg["gaussian_process_denoising"]: for i in range(num_images): if np.random.rand() < self.cfg["gaussian_process_rate"]: - train_image = image_arr[i,:,:,0] - gp_noise = ss.norm.rvs(scale=self.gp_sigma, size=self.gp_num_pix).reshape(self.gp_sample_height, self.gp_sample_width) - gp_noise = sm.imresize(gp_noise, self.gp_rescale_factor, interp="bicubic", mode="F") + train_image = image_arr[i, :, :, 0] + gp_noise = ss.norm.rvs(scale=self.gp_sigma, + size=self.gp_num_pix).reshape( + self.gp_sample_height, + self.gp_sample_width) + gp_noise = sm.imresize(gp_noise, + self.gp_rescale_factor, + interp="bicubic", + mode="F") train_image[train_image > 0] += gp_noise[train_image > 0] - image_arr[i,:,:,0] = train_image + image_arr[i, :, :, 0] = train_image # Symmetrize images. if self.cfg["symmetrize"]: for i in range(num_images): - train_image = image_arr[i,:,:,0] + train_image = image_arr[i, :, :, 0] # Rotate with 50% probability. if np.random.rand() < 0.5: theta = 180.0 - rot_map = cv2.getRotationMatrix2D(tuple(self.im_center), theta, 1) - train_image = cv2.warpAffine(train_image, rot_map, (self.im_height, self.im_width), flags=cv2.INTER_NEAREST) + rot_map = cv2.getRotationMatrix2D(tuple(self.im_center), + theta, 1) + train_image = cv2.warpAffine( + train_image, + rot_map, (self.im_height, self.im_width), + flags=cv2.INTER_NEAREST) if self.gripper_mode == GripperMode.LEGACY_SUCTION: - pose_arr[:,3] = -pose_arr[:,3] + pose_arr[:, 3] = -pose_arr[:, 3] elif self.gripper_mode == GripperMode.SUCTION: - pose_arr[:,4] = -pose_arr[:,4] + pose_arr[:, 4] = -pose_arr[:, 4] # Reflect left right with 50% probability. if np.random.rand() < 0.5: train_image = np.fliplr(train_image) @@ -1309,10 +1618,10 @@ def _distort(self, image_arr, pose_arr): train_image = np.flipud(train_image) if self.gripper_mode == GripperMode.LEGACY_SUCTION: - pose_arr[:,3] = -pose_arr[:,3] + pose_arr[:, 3] = -pose_arr[:, 3] elif self.gripper_mode == GripperMode.SUCTION: - pose_arr[:,4] = -pose_arr[:,4] - image_arr[i,:,:,0] = train_image + pose_arr[:, 4] = -pose_arr[:, 4] + image_arr[i, :, :, 0] = train_image return image_arr, pose_arr def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): @@ -1341,17 +1650,17 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): raw_poses = np.array(poses, copy=True) labels = self.dataset.tensor(self.label_field_name, i).arr - # If no datapoints from this file are in validation then just continue. + # If no datapoints from this file are in validation then just + # continue. if validation_set: indices = self.val_index_map[i] else: - indices = self.train_index_map[i] + indices = self.train_index_map[i] if len(indices) == 0: continue - images = images[indices,...] - poses = read_pose_data(poses[indices,:], - self.gripper_mode) + images = images[indices, ...] + poses = read_pose_data(poses[indices, :], self.gripper_mode) raw_poses = raw_poses[indices, :] labels = labels[indices] @@ -1360,7 +1669,8 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): labels = labels.astype(np.uint8) if self._angular_bins > 0: - # Form mask to extract predictions from ground-truth angular bins. + # Form mask to extract predictions from ground-truth angular + # bins. angles = raw_poses[:, 3] neg_ind = np.where(angles < 0) angles = np.abs(angles) % self._max_angle @@ -1369,24 +1679,28 @@ def _error_rate_in_batches(self, num_files_eval=None, validation_set=True): l_neg_90 = np.where(angles < (-1 * (self._max_angle / 2))) angles[g_90] -= self._max_angle angles[l_neg_90] += self._max_angle - angles *= -1 # Hack to fix reverse angle convention. - # TODO(vsatish): Actually fix this. + # TODO(vsatish): Actually fix this. + angles *= -1 # Hack to fix reverse angle convention. angles += (self._max_angle / 2) - pred_mask = np.zeros((labels.shape[0], self._angular_bins*2), dtype=bool) + pred_mask = np.zeros((labels.shape[0], self._angular_bins * 2), + dtype=bool) for i in range(angles.shape[0]): - pred_mask[i, int((angles[i] // self._bin_width)*2)] = True - pred_mask[i, int((angles[i] // self._bin_width)*2 + 1)] = True - + pred_mask[i, int( + (angles[i] // self._bin_width) * 2)] = True + pred_mask[i, + int((angles[i] // self._bin_width) * 2 + + 1)] = True + # Get predictions. predictions = self.gqcnn.predict(images, poses) if self._angular_bins > 0: - predictions = predictions[pred_mask].reshape((-1, 2)) + predictions = predictions[pred_mask].reshape((-1, 2)) # Update. - all_predictions.extend(predictions[:,1].tolist()) + all_predictions.extend(predictions[:, 1].tolist()) all_labels.extend(labels.tolist()) - + # Get learning result. result = None if self.training_mode == TrainingMode.CLASSIFICATION: diff --git a/gqcnn/utils/__init__.py b/gqcnn/utils/__init__.py index 37aaf47c..c4d50b96 100644 --- a/gqcnn/utils/__init__.py +++ b/gqcnn/utils/__init__.py @@ -26,19 +26,18 @@ from __future__ import division from __future__ import print_function -from .enums import (ImageMode, TrainingMode, GripperMode, - InputDepthMode, GeneralConstants, - GQCNNTrainingStatus, GQCNNFilenames) +from .enums import (ImageMode, TrainingMode, GripperMode, InputDepthMode, + GeneralConstants, GQCNNTrainingStatus, GQCNNFilenames) from .policy_exceptions import (NoValidGraspsException, NoAntipodalPairsFoundException) from .train_stats_logger import TrainStatsLogger -from .utils import (is_py2, set_cuda_visible_devices, pose_dim, - read_pose_data, reduce_shape, - weight_name_to_layer_name) +from .utils import (is_py2, set_cuda_visible_devices, pose_dim, read_pose_data, + reduce_shape, weight_name_to_layer_name) -__all__ = ["is_py2", "set_cuda_visible_devices", "pose_dim", "read_pose_data", "reduce_shape", - "weight_name_to_layer_name", "ImageMode", "TrainingMode", - "GripperMode", "InputDepthMode", "GeneralConstants", "GQCNNTrainingStatus", - "NoValidGraspsException", "NoAntipodalPairsFoundException", - "TrainStatsLogger", - "GQCNNFilenames"] +__all__ = [ + "is_py2", "set_cuda_visible_devices", "pose_dim", "read_pose_data", + "reduce_shape", "weight_name_to_layer_name", "ImageMode", "TrainingMode", + "GripperMode", "InputDepthMode", "GeneralConstants", "GQCNNTrainingStatus", + "NoValidGraspsException", "NoAntipodalPairsFoundException", + "TrainStatsLogger", "GQCNNFilenames" +] diff --git a/gqcnn/utils/enums.py b/gqcnn/utils/enums.py index 30d7d74e..0cce78a7 100644 --- a/gqcnn/utils/enums.py +++ b/gqcnn/utils/enums.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Constants/enums. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division @@ -33,18 +36,19 @@ import tensorflow as tf + # Other constants. class GeneralConstants(object): SEED = 3472134 - SEED_SAMPLE_MAX = 2**32 - 1 # Max range for seed (at least for `np.random.seed`). + SEED_SAMPLE_MAX = 2**32 - 1 # Max range for `np.random.seed`. timeout_option = tf.RunOptions(timeout_in_ms=1000000) - JSON_INDENT = 2 MAX_PREFETCH_Q_SIZE = 250 NUM_PREFETCH_Q_WORKERS = 3 QUEUE_SLEEP = 0.001 PI = math.pi - FIGSIZE = 16 # For visualization. - + FIGSIZE = 16 # For visualization. + + # Enum for image modalities. class ImageMode(object): BINARY = "binary" @@ -55,11 +59,13 @@ class ImageMode(object): DEPTH_TF = "depth_tf" DEPTH_TF_TABLE = "depth_tf_table" TF_DEPTH_IMS = "tf_depth_ims" - + + # Enum for training modes. class TrainingMode(object): CLASSIFICATION = "classification" - REGRESSION = "regression" # Has not been shown to work, for experimentation only! + REGRESSION = "regression" # Has not been tested, for experimentation only! + # Enum for input pose data formats. class GripperMode(object): @@ -69,18 +75,21 @@ class GripperMode(object): LEGACY_PARALLEL_JAW = "legacy_parallel_jaw" LEGACY_SUCTION = "legacy_suction" + # Enum for input depth mode. class InputDepthMode(object): POSE_STREAM = "pose_stream" SUB = "im_depth_sub" IM_ONLY = "im_only" + # Enum for training status. class GQCNNTrainingStatus(object): NOT_STARTED = "not_started" SETTING_UP = "setting_up" TRAINING = "training" + # Enum for filenames. class GQCNNFilenames(object): PCT_POS_VAL = "pct_pos_val.npy" diff --git a/gqcnn/utils/policy_exceptions.py b/gqcnn/utils/policy_exceptions.py index 8aa7aff2..db02de7b 100644 --- a/gqcnn/utils/policy_exceptions.py +++ b/gqcnn/utils/policy_exceptions.py @@ -22,22 +22,29 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -Exceptions that can be thrown by sub-classes of GraspingPolicy. -Author: Vishal Satish +Exceptions that can be thrown by sub-classes of `GraspingPolicy`. + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division from __future__ import print_function + class NoValidGraspsException(Exception): """Exception for when antipodal point pairs can be found in the depth - image but none are valid grasps that can be executed.""" - def __init__(self, in_collision=True, not_confident=False, *args, **kwargs): + image but none are valid grasps that can be executed.""" + + def __init__(self, in_collision=True, not_confident=False, *args, + **kwargs): self.in_collision = in_collision self.not_confident = not_confident Exception.__init__(self, *args, **kwargs) + class NoAntipodalPairsFoundException(Exception): """Exception for when no antipodal point pairs can be found in the depth - image.""" + image.""" pass diff --git a/gqcnn/utils/train_stats_logger.py b/gqcnn/utils/train_stats_logger.py index 6573edeb..814c7f74 100644 --- a/gqcnn/utils/train_stats_logger.py +++ b/gqcnn/utils/train_stats_logger.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Handles logging of various training statistics. -Author: Vishal Satish + +Author +------ +Vishal Satish """ from __future__ import absolute_import from __future__ import division @@ -35,6 +38,7 @@ from .enums import GQCNNFilenames + class TrainStatsLogger(object): """Logger for training statistics.""" @@ -43,7 +47,7 @@ def __init__(self, experiment_dir): Parameters ---------- experiment_dir : str - the experiment directory to save statistics to + The experiment directory to save statistics to. """ self.experiment_dir = experiment_dir self.train_eval_iters = [] @@ -59,50 +63,28 @@ def __init__(self, experiment_dir): def log(self): """Flush all of the statistics to the given experiment directory.""" + np.save(os.path.join(self.experiment_dir, GQCNNFilenames.TRAIN_ITERS), + self.train_eval_iters) + np.save(os.path.join(self.experiment_dir, GQCNNFilenames.TRAIN_LOSSES), + self.train_losses) + np.save(os.path.join(self.experiment_dir, GQCNNFilenames.TRAIN_ERRORS), + self.train_errors) np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.TRAIN_ITERS), - self.train_eval_iters) - np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.TRAIN_LOSSES), - self.train_losses) - np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.TRAIN_ERRORS), - self.train_errors) - np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.TOTAL_TRAIN_ERRORS), + os.path.join(self.experiment_dir, + GQCNNFilenames.TOTAL_TRAIN_ERRORS), self.total_train_errors) np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.TOTAL_TRAIN_LOSSES), + os.path.join(self.experiment_dir, + GQCNNFilenames.TOTAL_TRAIN_LOSSES), self.total_train_losses) + np.save(os.path.join(self.experiment_dir, GQCNNFilenames.VAL_ITERS), + self.val_eval_iters) + np.save(os.path.join(self.experiment_dir, GQCNNFilenames.VAL_LOSSES), + self.val_losses) + np.save(os.path.join(self.experiment_dir, GQCNNFilenames.VAL_ERRORS), + self.val_errors) np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.VAL_ITERS), - self.val_eval_iters) - np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.VAL_LOSSES), - self.val_losses) - np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.VAL_ERRORS), - self.val_errors) - np.save( - os.path.join( - self.experiment_dir, - GQCNNFilenames.LEARNING_RATES), + os.path.join(self.experiment_dir, GQCNNFilenames.LEARNING_RATES), self.learning_rates) def update(self, **stats): @@ -115,7 +97,7 @@ def update(self, **stats): Parameters ---------- stats : dict - dict of statistics to be updated + Dict of statistics to be updated. """ for stat, val in stats.items(): if stat == "train_eval_iter": diff --git a/gqcnn/utils/utils.py b/gqcnn/utils/utils.py index 3e1e562a..349a9016 100644 --- a/gqcnn/utils/utils.py +++ b/gqcnn/utils/utils.py @@ -22,8 +22,11 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -Simple utility functions -Authors: Jeff Mahler, Vishal Satish, Lucas Manuelli +Simple utility functions. + +Authors +------- +Jeff Mahler, Vishal Satish, Lucas Manuelli """ from __future__ import absolute_import from __future__ import division @@ -41,21 +44,23 @@ # Set up logger. logger = Logger.get_logger("gqcnn/utils/utils.py") + def is_py2(): - return sys.version[0] == "2" + return sys.version[0] == "2" + def set_cuda_visible_devices(gpu_list): """Sets CUDA_VISIBLE_DEVICES environment variable to only show certain - gpus. + gpus. Note ---- - If gpu_list is empty does nothing + If gpu_list is empty does nothing. Parameters ---------- - gpu_list : list - list of gpus to set as visible + gpu_list : list + List of gpus to set as visible. """ if len(gpu_list) == 0: return @@ -64,23 +69,25 @@ def set_cuda_visible_devices(gpu_list): for gpu in gpu_list: cuda_visible_devices += str(gpu) + "," - logger.info("Setting CUDA_VISIBLE_DEVICES = {}".format(cuda_visible_devices)) + logger.info( + "Setting CUDA_VISIBLE_DEVICES = {}".format(cuda_visible_devices)) os.environ["CUDA_VISIBLE_DEVICES"] = cuda_visible_devices + def pose_dim(gripper_mode): """Returns the dimensions of the pose vector for the given - gripper mode. - + gripper mode. + Parameters ---------- gripper_mode: :obj:`GripperMode` - enum for gripper mode, see optimizer_constants.py for all - possible gripper modes + Enum for gripper mode, see optimizer_constants.py for all possible + gripper modes. Returns ------- :obj:`numpy.ndarray` - sliced pose_data corresponding to gripper mode + Sliced pose_data corresponding to gripper mode. """ if gripper_mode == GripperMode.PARALLEL_JAW: return 1 @@ -93,58 +100,63 @@ def pose_dim(gripper_mode): elif gripper_mode == GripperMode.LEGACY_SUCTION: return 2 else: - raise ValueError("Gripper mode \"{}\" not supported.".format(gripper_mode)) - + raise ValueError( + "Gripper mode \"{}\" not supported.".format(gripper_mode)) + + def read_pose_data(pose_arr, gripper_mode): """Read the pose data and slice it according to the specified gripper mode. - + Parameters ---------- pose_arr: :obj:`numpy.ndarray` - full pose data array read in from file + Full pose data array read in from file. gripper_mode: :obj:`GripperMode` - enum for gripper mode, see optimizer_constants.py for all - possible gripper modes + Enum for gripper mode, see optimizer_constants.py for all possible + gripper modes. Returns ------- :obj:`numpy.ndarray` - sliced pose_data corresponding to input data mode + Sliced pose_data corresponding to input data mode. """ if gripper_mode == GripperMode.PARALLEL_JAW: if pose_arr.ndim == 1: return pose_arr[2:3] else: - return pose_arr[:,2:3] + return pose_arr[:, 2:3] elif gripper_mode == GripperMode.SUCTION: if pose_arr.ndim == 1: return np.r_[pose_arr[2], pose_arr[4]] else: - return np.c_[pose_arr[:,2], pose_arr[:,4]] + return np.c_[pose_arr[:, 2], pose_arr[:, 4]] elif gripper_mode == GripperMode.MULTI_SUCTION: if pose_arr.ndim == 1: return pose_arr[2:3] else: - return pose_arr[:,2:3] + return pose_arr[:, 2:3] elif gripper_mode == GripperMode.LEGACY_PARALLEL_JAW: if pose_arr.ndim == 1: return pose_arr[2:3] else: - return pose_arr[:,2:3] + return pose_arr[:, 2:3] elif gripper_mode == GripperMode.LEGACY_SUCTION: if pose_arr.ndim == 1: return pose_arr[2:4] else: - return pose_arr[:,2:4] + return pose_arr[:, 2:4] else: - raise ValueError("Gripper mode \"{}\" not supported.".format(gripper_mode)) + raise ValueError( + "Gripper mode \"{}\" not supported.".format(gripper_mode)) + def reduce_shape(shape): """Get shape of a layer for flattening.""" shape = [x.value for x in shape[1:]] - f = lambda x, y: 1 if y is None else x * y + f = lambda x, y: 1 if y is None else x * y # noqa: E731 return reduce(f, shape, 1) + def weight_name_to_layer_name(weight_name): """Convert the name of weights to the layer name.""" tokens = weight_name.split("_") @@ -153,8 +165,8 @@ def weight_name_to_layer_name(weight_name): # Modern naming convention. if type_name == "weights" or type_name == "bias": if len(tokens) >= 3 and tokens[-3] == "input": - return weight_name[:weight_name.rfind("input")-1] - return weight_name[:weight_name.rfind(type_name)-1] + return weight_name[:weight_name.rfind("input") - 1] + return weight_name[:weight_name.rfind(type_name) - 1] # Legacy. if type_name == "im": return weight_name[:-4] diff --git a/ros_nodes/grasp_planner_node.py b/ros_nodes/grasp_planner_node.py index 06cdfcfe..71c2103a 100755 --- a/ros_nodes/grasp_planner_node.py +++ b/ros_nodes/grasp_planner_node.py @@ -23,7 +23,7 @@ HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -ROS Server for planning GQ-CNN grasps. +ROS Server for planning GQ-CNN grasps. Author ----- @@ -43,16 +43,20 @@ import rospy from autolab_core import YamlConfig -from perception import CameraIntrinsics, ColorImage, DepthImage, BinaryImage, RgbdImage +from perception import (CameraIntrinsics, ColorImage, DepthImage, BinaryImage, + RgbdImage) from visualization import Visualizer2D as vis -from gqcnn.grasping import Grasp2D, SuctionPoint2D, CrossEntropyRobustGraspingPolicy, RgbdImageState +from gqcnn.grasping import (Grasp2D, SuctionPoint2D, + CrossEntropyRobustGraspingPolicy, RgbdImageState) from gqcnn.utils import GripperMode, NoValidGraspsException from geometry_msgs.msg import PoseStamped from std_msgs.msg import Header -from gqcnn.srv import GQCNNGraspPlanner, GQCNNGraspPlannerBoundingBox, GQCNNGraspPlannerSegmask +from gqcnn.srv import (GQCNNGraspPlanner, GQCNNGraspPlannerBoundingBox, + GQCNNGraspPlannerSegmask) from gqcnn.msg import GQCNNGrasp + class GraspPlanner(object): def __init__(self, cfg, cv_bridge, grasping_policy, grasp_pose_publisher): """ @@ -74,15 +78,20 @@ def __init__(self, cfg, cv_bridge, grasping_policy, grasp_pose_publisher): # Set minimum input dimensions. self.pad = max( - math.ceil(np.sqrt(2) * (float(self.cfg["policy"]["metric"]["crop_width"]) / 2)), - math.ceil(np.sqrt(2) * (float(self.cfg["policy"]["metric"]["crop_height"]) / 2)) - ) - self.min_width = 2 * self.pad + self.cfg["policy"]["metric"]["crop_width"] - self.min_height = 2 * self.pad + self.cfg["policy"]["metric"]["crop_height"] + math.ceil( + np.sqrt(2) * + (float(self.cfg["policy"]["metric"]["crop_width"]) / 2)), + math.ceil( + np.sqrt(2) * + (float(self.cfg["policy"]["metric"]["crop_height"]) / 2))) + self.min_width = 2 * self.pad + self.cfg["policy"]["metric"][ + "crop_width"] + self.min_height = 2 * self.pad + self.cfg["policy"]["metric"][ + "crop_height"] def read_images(self, req): """Reads images from a ROS service request. - + Parameters --------- req: :obj:`ROS ServiceRequest` @@ -94,34 +103,51 @@ def read_images(self, req): # Get the raw camera info as ROS `CameraInfo`. raw_camera_info = req.camera_info - - # Wrap the camera info in a BerkeleyAutomation/perception `CameraIntrinsics` object. - camera_intr = CameraIntrinsics(raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width) - # Create wrapped BerkeleyAutomation/perception RGB and depth images by unpacking the ROS images using ROS `CvBridge` + # Wrap the camera info in a BerkeleyAutomation/perception + # `CameraIntrinsics` object. + camera_intr = CameraIntrinsics( + raw_camera_info.header.frame_id, raw_camera_info.K[0], + raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], + raw_camera_info.K[1], raw_camera_info.height, + raw_camera_info.width) + + # Create wrapped BerkeleyAutomation/perception RGB and depth images by + # unpacking the ROS images using ROS `CvBridge` try: - color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2(raw_color, "rgb8"), frame=camera_intr.frame) - depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2(raw_depth, desired_encoding = "passthrough"), frame=camera_intr.frame) + color_im = ColorImage(self.cv_bridge.imgmsg_to_cv2( + raw_color, "rgb8"), + frame=camera_intr.frame) + depth_im = DepthImage(self.cv_bridge.imgmsg_to_cv2( + raw_depth, desired_encoding="passthrough"), + frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) # Check image sizes. if color_im.height != depth_im.height or \ color_im.width != depth_im.width: - msg = "Color image and depth image must be the same shape! Color is %d x %d but depth is %d x %d" %(color_im.height, color_im.width, depth_im.height, depth_im.width) + msg = ("Color image and depth image must be the same shape! Color" + " is %d x %d but depth is %d x %d") % ( + color_im.height, color_im.width, depth_im.height, + depth_im.width) rospy.logerr(msg) - raise rospy.ServiceException(msg) + raise rospy.ServiceException(msg) - if color_im.height < self.min_height or color_im.width < self.min_width: - msg = "Color image is too small! Must be at least %d x %d resolution but the requested image is only %d x %d" %(self.min_height, self.min_width, color_im.height, color_im.width) + if (color_im.height < self.min_height + or color_im.width < self.min_width): + msg = ("Color image is too small! Must be at least %d x %d" + " resolution but the requested image is only %d x %d") % ( + self.min_height, self.min_width, color_im.height, + color_im.width) rospy.logerr(msg) raise rospy.ServiceException(msg) return color_im, depth_im, camera_intr - + def plan_grasp(self, req): """Grasp planner request handler. - + Parameters --------- req: :obj:`ROS ServiceRequest` @@ -132,18 +158,21 @@ def plan_grasp(self, req): def plan_grasp_bb(self, req): """Grasp planner request handler. - + Parameters --------- req: :obj:`ROS ServiceRequest` `ROS ServiceRequest` for grasp planner service. """ color_im, depth_im, camera_intr = self.read_images(req) - return self._plan_grasp(color_im, depth_im, camera_intr, bounding_box=req.bounding_box) + return self._plan_grasp(color_im, + depth_im, + camera_intr, + bounding_box=req.bounding_box) def plan_grasp_segmask(self, req): """Grasp planner request handler. - + Parameters --------- req: :obj:`ROS ServiceRequest` @@ -152,24 +181,33 @@ def plan_grasp_segmask(self, req): color_im, depth_im, camera_intr = self.read_images(req) raw_segmask = req.segmask try: - segmask = BinaryImage(self.cv_bridge.imgmsg_to_cv2(raw_segmask, desired_encoding = "passthrough"), frame=camera_intr.frame) + segmask = BinaryImage(self.cv_bridge.imgmsg_to_cv2( + raw_segmask, desired_encoding="passthrough"), + frame=camera_intr.frame) except CvBridgeError as cv_bridge_exception: rospy.logerr(cv_bridge_exception) if color_im.height != segmask.height or \ color_im.width != segmask.width: - msg = "Images and segmask must be the same shape! Color image is %d x %d but segmask is %d x %d" %(color_im.height, color_im.width, segmask.height, segmask.width) + msg = ("Images and segmask must be the same shape! Color image is" + " %d x %d but segmask is %d x %d") % ( + color_im.height, color_im.width, segmask.height, + segmask.width) rospy.logerr(msg) - raise rospy.ServiceException(msg) - - return self._plan_grasp(color_im, depth_im, camera_intr, segmask=segmask) - - def _plan_grasp(self, color_im, - depth_im, - camera_intr, - bounding_box=None, - segmask=None): + raise rospy.ServiceException(msg) + + return self._plan_grasp(color_im, + depth_im, + camera_intr, + segmask=segmask) + + def _plan_grasp(self, + color_im, + depth_im, + camera_intr, + bounding_box=None, + segmask=None): """Grasp planner request handler. - + Parameters --------- req: :obj:`ROS ServiceRequest` @@ -178,13 +216,17 @@ def _plan_grasp(self, color_im, rospy.loginfo("Planning Grasp") # Inpaint images. - color_im = color_im.inpaint(rescale_factor=self.cfg["inpaint_rescale_factor"]) - depth_im = depth_im.inpaint(rescale_factor=self.cfg["inpaint_rescale_factor"]) + color_im = color_im.inpaint( + rescale_factor=self.cfg["inpaint_rescale_factor"]) + depth_im = depth_im.inpaint( + rescale_factor=self.cfg["inpaint_rescale_factor"]) # Init segmask. if segmask is None: - segmask = BinaryImage(255*np.ones(depth_im.shape).astype(np.uint8), frame=color_im.frame) - + segmask = BinaryImage(255 * + np.ones(depth_im.shape).astype(np.uint8), + frame=color_im.frame) + # Visualize. if self.cfg["vis"]["color_image"]: vis.imshow(color_im) @@ -196,10 +238,10 @@ def _plan_grasp(self, color_im, vis.imshow(segmask) vis.show() - # Aggregate color and depth images into a single BerkeleyAutomation/perception `RgbdImage`. + # Aggregate color and depth images into a single + # BerkeleyAutomation/perception `RgbdImage`. rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) - # Mask bounding box. if bounding_box is not None: # Calc bb parameters. @@ -208,7 +250,8 @@ def _plan_grasp(self, color_im, max_x = bounding_box.maxX max_y = bounding_box.maxY - # Contain box to image->don't let it exceed image height/width bounds. + # Contain box to image->don't let it exceed image height/width + # bounds. if min_x < 0: min_x = 0 if min_y < 0: @@ -219,36 +262,43 @@ def _plan_grasp(self, color_im, max_y = rgbd_im.height # Mask. - bb_segmask_arr = np.zeros([rgbd_image.height, rgbd_image.width]) + bb_segmask_arr = np.zeros([rgbd_im.height, rgbd_im.width]) bb_segmask_arr[min_y:max_y, min_x:max_x] = 255 - bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8), segmask.frame) + bb_segmask = BinaryImage(bb_segmask_arr.astype(np.uint8), + segmask.frame) segmask = segmask.mask_binary(bb_segmask) - - # Visualize. + + # Visualize. if self.cfg["vis"]["rgbd_state"]: masked_rgbd_im = rgbd_im.mask_binary(segmask) vis.figure() - vis.subplot(1,2,1) + vis.subplot(1, 2, 1) vis.imshow(masked_rgbd_im.color) - vis.subplot(1,2,2) + vis.subplot(1, 2, 2) vis.imshow(masked_rgbd_im.depth) vis.show() - # Create an `RgbdImageState` with the cropped `RgbdImage` and `CameraIntrinsics`. - rgbd_state = RgbdImageState(rgbd_im, - camera_intr, - segmask=segmask) - + # Create an `RgbdImageState` with the cropped `RgbdImage` and + # `CameraIntrinsics`. + rgbd_state = RgbdImageState(rgbd_im, camera_intr, segmask=segmask) + # Execute policy. try: - return self.execute_policy(rgbd_state, self.grasping_policy, self.grasp_pose_publisher, camera_intr.frame) + return self.execute_policy(rgbd_state, self.grasping_policy, + self.grasp_pose_publisher, + camera_intr.frame) except NoValidGraspsException: - rospy.logerr("While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!") - raise rospy.ServiceException("While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!") - - def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher, pose_frame): + rospy.logerr( + ("While executing policy found no valid grasps from sampled" + " antipodal point pairs. Aborting Policy!")) + raise rospy.ServiceException( + ("While executing policy found no valid grasps from sampled" + " antipodal point pairs. Aborting Policy!")) + + def execute_policy(self, rgbd_image_state, grasping_policy, + grasp_pose_publisher, pose_frame): """Executes a grasping policy on an `RgbdImageState`. - + Parameters ---------- rgbd_image_state: :obj:`RgbdImageState` @@ -264,7 +314,7 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher # Execute the policy"s action. grasp_planning_start_time = time.time() grasp = grasping_policy(rgbd_image_state) - + # Create `GQCNNGrasp` return msg and populate it. gqcnn_grasp = GQCNNGrasp() gqcnn_grasp.q_value = grasp.q_value @@ -283,8 +333,9 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher gqcnn_grasp.angle = grasp.grasp.angle gqcnn_grasp.depth = grasp.grasp.depth gqcnn_grasp.thumbnail = grasp.image.rosmsg - - # Create and publish the pose alone for easy visualization of grasp pose in Rviz. + + # Create and publish the pose alone for easy visualization of grasp + # pose in Rviz. pose_stamped = PoseStamped() pose_stamped.pose = grasp.grasp.pose().pose_msg header = Header() @@ -294,12 +345,13 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher grasp_pose_publisher.publish(pose_stamped) # Return `GQCNNGrasp` msg. - rospy.loginfo("Total grasp planning time: " + str(time.time() - grasp_planning_start_time) + " secs.") + rospy.loginfo("Total grasp planning time: " + + str(time.time() - grasp_planning_start_time) + " secs.") return gqcnn_grasp + if __name__ == "__main__": - # Initialize the ROS node. rospy.init_node("Grasp_Sampler_Server") @@ -317,30 +369,31 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher try: gqcnn_config = model_config["gqcnn"] gripper_mode = gqcnn_config["gripper_mode"] - except: + except KeyError: gqcnn_config = model_config["gqcnn_config"] input_data_mode = gqcnn_config["input_data_mode"] if input_data_mode == "tf_image": gripper_mode = GripperMode.LEGACY_PARALLEL_JAW elif input_data_mode == "tf_image_suction": - gripper_mode = GripperMode.LEGACY_SUCTION + gripper_mode = GripperMode.LEGACY_SUCTION elif input_data_mode == "suction": - gripper_mode = GripperMode.SUCTION + gripper_mode = GripperMode.SUCTION elif input_data_mode == "multi_suction": - gripper_mode = GripperMode.MULTI_SUCTION + gripper_mode = GripperMode.MULTI_SUCTION elif input_data_mode == "parallel_jaw": gripper_mode = GripperMode.PARALLEL_JAW else: - raise ValueError("Input data mode {} not supported!".format(input_data_mode)) + raise ValueError( + "Input data mode {} not supported!".format(input_data_mode)) # Set config. config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/ros/gqcnn_suction.yaml") - if gripper_mode == GripperMode.LEGACY_PARALLEL_JAW or gripper_mode == GripperMode.PARALLEL_JAW: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/examples/ros/gqcnn_pj.yaml") + "..", "cfg/examples/ros/gqcnn_suction.yaml") + if (gripper_mode == GripperMode.LEGACY_PARALLEL_JAW + or gripper_mode == GripperMode.PARALLEL_JAW): + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/examples/ros/gqcnn_pj.yaml") # Read config. cfg = YamlConfig(config_filename) @@ -348,19 +401,27 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher policy_cfg["metric"]["gqcnn_model"] = model_dir # Create publisher to publish pose of final grasp. - grasp_pose_publisher = rospy.Publisher("/gqcnn_grasp/pose", PoseStamped, queue_size=10) + grasp_pose_publisher = rospy.Publisher("/gqcnn_grasp/pose", + PoseStamped, + queue_size=10) # Create a grasping policy. rospy.loginfo("Creating Grasping Policy") grasping_policy = CrossEntropyRobustGraspingPolicy(policy_cfg) # Create a grasp planner. - grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher) - - # Initialize the ROS service. - grasp_planning_service = rospy.Service("grasp_planner", GQCNNGraspPlanner, grasp_planner.plan_grasp) - grasp_planning_service_bb = rospy.Service("grasp_planner_bounding_box", GQCNNGraspPlannerBoundingBox, grasp_planner.plan_grasp_bb) - grasp_planning_service_segmask = rospy.Service("grasp_planner_segmask", GQCNNGraspPlannerSegmask, grasp_planner.plan_grasp_segmask) + grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, + grasp_pose_publisher) + + # Initialize the ROS service. + grasp_planning_service = rospy.Service("grasp_planner", GQCNNGraspPlanner, + grasp_planner.plan_grasp) + grasp_planning_service_bb = rospy.Service("grasp_planner_bounding_box", + GQCNNGraspPlannerBoundingBox, + grasp_planner.plan_grasp_bb) + grasp_planning_service_segmask = rospy.Service( + "grasp_planner_segmask", GQCNNGraspPlannerSegmask, + grasp_planner.plan_grasp_segmask) rospy.loginfo("Grasping Policy Initialized") # Spin forever. diff --git a/setup.py b/setup.py index c91b0974..e6535924 100644 --- a/setup.py +++ b/setup.py @@ -23,7 +23,10 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Setup of `gqcnn` Python codebase. -Author: Vishal Satish, Jeff Mahler + +Author +------ +Vishal Satish & Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -41,23 +44,33 @@ TF_MAX_VERSION = "1.13.1" # Set up logger. -logging.basicConfig() # Configure the root logger. +logging.basicConfig() # Configure the root logger. logger = logging.getLogger("setup.py") logger.setLevel(logging.INFO) + def get_tf_dep(): - # Check whether or not the Nvidia driver and GPUs are available and add the corresponding Tensorflow dependency. + # Check whether or not the Nvidia driver and GPUs are available and add the + # corresponding Tensorflow dependency. tf_dep = "tensorflow>={},<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) try: - gpus = subprocess.check_output(["nvidia-smi", "--query-gpu=gpu_name", "--format=csv"]).decode().strip().split("\n")[1:] + gpus = subprocess.check_output( + ["nvidia-smi", "--query-gpu=gpu_name", + "--format=csv"]).decode().strip().split("\n")[1:] if len(gpus) > 0: - tf_dep = "tensorflow-gpu>={},<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) + tf_dep = "tensorflow-gpu>={},<={}".format(TF_MIN_VERSION, + TF_MAX_VERSION) else: - logger.warning("Found Nvidia device driver but no devices...installing Tensorflow for CPU.") + no_device_msg = ("Found Nvidia device driver but no" + " devices...installing Tensorflow for CPU.") + logger.warning(no_device_msg) except OSError: - logger.warning("Could not find Nvidia device driver...installing Tensorflow for CPU.") + no_driver_msg = ("Could not find Nvidia device driver...installing" + " Tensorflow for CPU.") + logger.warning(no_driver_msg) return tf_dep + # TODO(vsatish): Use inheritance here. class DevelopCmd(develop): user_options_custom = [ @@ -78,14 +91,22 @@ def run(self): # Install Tensorflow dependency. if not self.docker: tf_dep = get_tf_dep() - subprocess.Popen([sys.executable, "-m", "pip", "install", tf_dep]).wait() + subprocess.Popen([sys.executable, "-m", "pip", "install", + tf_dep]).wait() else: - # If we"re using Docker, this will already have been installed explicitly through the correct `{cpu/gpu}_requirements.txt`; there is no way to check for CUDA/GPUs at Docker build time because there is no easy way to set the Nvidia runtime. - logger.warning("Omitting Tensorflow dependency because of Docker installation.") # TODO(vsatish): Figure out why this isn"t printed. + # If we're using Docker, this will already have been installed + # explicitly through the correct `{cpu/gpu}_requirements.txt`; + # there is no way to check for CUDA/GPUs at Docker build time + # because there is no easy way to set the Nvidia runtime. + # TODO(vsatish): Figure out why this isn't printed. + skip_tf_msg = ("Omitting Tensorflow dependency because of Docker" + " installation.") + logger.warning(skip_tf_msg) # Run installation. develop.run(self) + class InstallCmd(install, object): user_options_custom = [ ("docker", None, "installing in Docker"), @@ -105,55 +126,55 @@ def run(self): # Install Tensorflow dependency. if not self.docker: tf_dep = get_tf_dep() - subprocess.Popen([sys.executable, "-m", "pip", "install", tf_dep]).wait() + subprocess.Popen([sys.executable, "-m", "pip", "install", + tf_dep]).wait() else: - # If we"re using Docker, this will already have been installed explicitly through the correct `{cpu/gpu}_requirements.txt`; there is no way to check for CUDA/GPUs at Docker build time because there is no easy way to set the Nvidia runtime. - logger.warning("Omitting Tensorflow dependency because of Docker installation.") #TODO (vsatish): Figure out why this isn"t printed. + # If we're using Docker, this will already have been installed + # explicitly through the correct `{cpu/gpu}_requirements.txt`; + # there is no way to check for CUDA/GPUs at Docker build time + # because there is no easy way to set the Nvidia runtime. + # TODO (vsatish): Figure out why this isn't printed. + skip_tf_msg = ("Omitting Tensorflow dependency because of Docker" + " installation.") + logger.warning(skip_tf_msg) # Run installation. install.run(self) + requirements = [ - "autolab-core", - "autolab-perception", - "visualization", - "numpy", - "scipy", - "matplotlib", - "opencv-python", - "scikit-image", - "scikit-learn", - "psutil", + "autolab-core", "autolab-perception", "visualization", "numpy", "scipy", + "matplotlib", "opencv-python", "scikit-image", "scikit-learn", "psutil", "gputil" ] -exec(open(os.path.join(os.path.dirname(os.path.realpath(__file__)), "gqcnn/version.py")).read()) - -setup(name="gqcnn", - version=__version__, - description="Project code for running Grasp Quality Convolutional Neural Networks", - author="Vishal Satish", - author_email="vsatish@berkeley.edu", - license = "Berkeley Copyright", - url = "https://github.com/BerkeleyAutomation/gqcnn", - keywords = "robotics grasping vision deep learning", - classifiers = [ - "Development Status :: 4 - Beta", - "Programming Language :: Python :: 2.7", - "Programming Language :: Python :: 3", - "Natural Language :: English", - "Topic :: Scientific/Engineering" - ], - packages=find_packages(), - install_requires = requirements, - extras_require = { "docs" : [ - "sphinx", - "sphinxcontrib-napoleon", - "sphinx_rtd_theme" - ], - }, - cmdclass={ +exec( + open( + os.path.join(os.path.dirname(os.path.realpath(__file__)), + "gqcnn/version.py")).read()) + +setup( + name="gqcnn", + version=__version__, # noqa F821 + description=("Project code for running Grasp Quality Convolutional" + " Neural Networks"), + author="Vishal Satish", + author_email="vsatish@berkeley.edu", + license="Berkeley Copyright", + url="https://github.com/BerkeleyAutomation/gqcnn", + keywords="robotics grasping vision deep learning", + classifiers=[ + "Development Status :: 4 - Beta", + "Programming Language :: Python :: 2.7", + "Programming Language :: Python :: 3", "Natural Language :: English", + "Topic :: Scientific/Engineering" + ], + packages=find_packages(), + install_requires=requirements, + extras_require={ + "docs": ["sphinx", "sphinxcontrib-napoleon", "sphinx_rtd_theme"], + }, + cmdclass={ "install": InstallCmd, "develop": DevelopCmd - } -) + }) diff --git a/tools/analyze_gqcnn_performance.py b/tools/analyze_gqcnn_performance.py index 021fb43b..64c6b51b 100644 --- a/tools/analyze_gqcnn_performance.py +++ b/tools/analyze_gqcnn_performance.py @@ -26,7 +26,7 @@ Author ------ -Vishal Satish and Jeff Mahler +Vishal Satish & Jeff Mahler """ from __future__ import absolute_import from __future__ import division @@ -34,8 +34,6 @@ import argparse import os -import sys -import time from autolab_core import YamlConfig, Logger from gqcnn import GQCNNAnalyzer @@ -45,12 +43,30 @@ if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Analyze a Grasp Quality Convolutional Neural Network with TensorFlow") - parser.add_argument("model_name", type=str, default=None, help="name of model to analyze") - parser.add_argument("--output_dir", type=str, default=None, help="path to save the analysis") - parser.add_argument("--dataset_config_filename", type=str, default=None, help="path to a configuration file for testing on a custom dataset") - parser.add_argument("--config_filename", type=str, default=None, help="path to the configuration file to use") - parser.add_argument("--model_dir", type=str, default=None, help="path to the model") + parser = argparse.ArgumentParser( + description=("Analyze a Grasp Quality Convolutional Neural Network" + " with TensorFlow")) + parser.add_argument("model_name", + type=str, + default=None, + help="name of model to analyze") + parser.add_argument("--output_dir", + type=str, + default=None, + help="path to save the analysis") + parser.add_argument( + "--dataset_config_filename", + type=str, + default=None, + help="path to a configuration file for testing on a custom dataset") + parser.add_argument("--config_filename", + type=str, + default=None, + help="path to the configuration file to use") + parser.add_argument("--model_dir", + type=str, + default=None, + help="path to the model") args = parser.parse_args() model_name = args.model_name output_dir = args.output_dir @@ -63,11 +79,12 @@ model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../models") model_dir = os.path.join(model_dir, model_name) - + # If `model_dir` contains many models, analyze all of them. model_dir = [model_dir] if "config.json" not in os.listdir(model_dir[0]): - logger.warning("Found multiple models in model_dir, analyzing all of them...") + logger.warning( + "Found multiple models in model_dir, analyzing all of them...") models = os.listdir(model_dir[0]) model_dir = [os.path.join(model_dir[0], model) for model in models] @@ -76,29 +93,30 @@ output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../analysis") if config_filename is None: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/tools/analyze_gqcnn_performance.yaml") + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/tools/analyze_gqcnn_performance.yaml") # Turn relative paths absolute. if not os.path.isabs(output_dir): output_dir = os.path.join(os.getcwd(), output_dir) if not os.path.isabs(config_filename): config_filename = os.path.join(os.getcwd(), config_filename) - if dataset_config_filename is not None and not os.path.isabs(dataset_config_filename): + if dataset_config_filename is not None and not os.path.isabs( + dataset_config_filename): config_filename = os.path.join(os.getcwd(), dataset_config_filename) # Make the output dir. if not os.path.exists(output_dir): os.mkdir(output_dir) - + # Read config. config = YamlConfig(config_filename) dataset_config = None if dataset_config_filename is not None: dataset_config = YamlConfig(dataset_config_filename) - + # Run the analyzer. analyzer = GQCNNAnalyzer(config, plot_backend="pdf") for model in model_dir: diff --git a/tools/finetune.py b/tools/finetune.py index 098d72f0..fa02df82 100644 --- a/tools/finetune.py +++ b/tools/finetune.py @@ -34,7 +34,6 @@ import argparse import os -import sys import time from autolab_core import YamlConfig, Logger @@ -47,28 +46,55 @@ if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Fine-Tune a pre-trained Grasp Quality Convolutional Neural Network with TensorFlow") - parser.add_argument("dataset_dir", type=str, default=None, - help="path to the dataset to use for training and validation") - parser.add_argument("base_model_name", type=str, default=None, + parser = argparse.ArgumentParser(description=( + "Fine-Tune a pre-trained Grasp Quality Convolutional Neural Network" + " with TensorFlow")) + parser.add_argument( + "dataset_dir", + type=str, + default=None, + help="path to the dataset to use for training and validation") + parser.add_argument("base_model_name", + type=str, + default=None, help="name of the pre-trained model to fine-tune") - parser.add_argument("--split_name", type=str, default="image_wise", + parser.add_argument("--split_name", + type=str, + default="image_wise", help="name of the split to train on") - parser.add_argument("--output_dir", type=str, default=None, + parser.add_argument("--output_dir", + type=str, + default=None, help="path to store the model") - parser.add_argument("--tensorboard_port", type=int, default=None, + parser.add_argument("--tensorboard_port", + type=int, + default=None, help="port to launch tensorboard on") - parser.add_argument("--seed", type=int, default=None, + parser.add_argument("--seed", + type=int, + default=None, help="random seed for training") - parser.add_argument("--config_filename", type=str, default=None, + parser.add_argument("--config_filename", + type=str, + default=None, help="path to the configuration file to use") - parser.add_argument("--model_dir", type=str, default=None, + parser.add_argument("--model_dir", + type=str, + default=None, help="path to the pre-trained model to fine-tune") - parser.add_argument("--name", type=str, default=None, + parser.add_argument("--name", + type=str, + default=None, help="name for the trained model") - parser.add_argument("--save_datetime", type=bool, default=False, - help="whether or not to save a model with the date and time of training") - parser.add_argument("--backend", type=str, default="tf", + parser.add_argument( + "--save_datetime", + type=bool, + default=False, + help=("whether or not to save a model with the date and time of" + " training")) + parser.add_argument("--backend", + type=str, + default="tf", help="the deep learning framework to use") args = parser.parse_args() dataset_dir = args.dataset_dir @@ -82,23 +108,23 @@ name = args.name save_datetime = args.save_datetime backend = args.backend - + # Set default output dir. if output_dir is None: output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../models") - + # Set default config filename. if config_filename is None: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/finetune.yaml") + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/finetune.yaml") # Set default model dir. if model_dir is None: model_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../models") - + # Turn relative paths absolute. if not os.path.isabs(dataset_dir): dataset_dir = os.path.join(os.getcwd(), dataset_dir) @@ -111,10 +137,10 @@ # Create full path to the pre-trained model. model_dir = os.path.join(model_dir, base_model_name) - + # Create output dir if necessary. utils.mkdir_safe(output_dir) - + # Open train config. train_config = YamlConfig(config_filename) if seed is not None: @@ -145,4 +171,5 @@ train_config, name=name) trainer.finetune(model_dir) - logger.info("Total Fine-tuning Time: " + str(utils.get_elapsed_time(time.time() - start_time))) + logger.info("Total Fine-tuning Time: " + + str(utils.get_elapsed_time(time.time() - start_time))) diff --git a/tools/hyperparam_search.py b/tools/hyperparam_search.py index 087b6562..907dcefa 100644 --- a/tools/hyperparam_search.py +++ b/tools/hyperparam_search.py @@ -34,26 +34,52 @@ from __future__ import print_function import argparse -import sys from autolab_core import YamlConfig, Logger from gqcnn import GQCNNSearch # Set up logger. -logger = Logger.get_logger("tools/hyperparam_search.py") +logger = Logger.get_logger("tools/hyperparam_search.py") if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Hyper-parameter search for GQ-CNN.") - parser.add_argument("datasets", nargs="+", default=None, help="path to datasets") - parser.add_argument("--base_model_dirs", nargs="+", default=[], help="path to pre-trained base models for fine-tuning") - parser.add_argument("--train_configs", nargs="+", default=["cfg/train.yaml"], help="path to training configs") - parser.add_argument("--analysis_config", type=str, default="cfg/tools/analyze_gqcnn_performance.yaml") - parser.add_argument("--split_names", nargs="+", default=["image_wise"], help="dataset splits to use") - parser.add_argument("--output_dir", type=str, default="models", help="path to store search data") - parser.add_argument("--search_name", type=str, default=None, help="name of search") - parser.add_argument("--cpu_cores", nargs="+", default=[], help="CPU cores to use") - parser.add_argument("--gpu_devices", nargs="+", default=[], help="GPU devices to use") + parser = argparse.ArgumentParser( + description="Hyper-parameter search for GQ-CNN.") + parser.add_argument("datasets", + nargs="+", + default=None, + help="path to datasets") + parser.add_argument("--base_model_dirs", + nargs="+", + default=[], + help="path to pre-trained base models for fine-tuning") + parser.add_argument("--train_configs", + nargs="+", + default=["cfg/train.yaml"], + help="path to training configs") + parser.add_argument("--analysis_config", + type=str, + default="cfg/tools/analyze_gqcnn_performance.yaml") + parser.add_argument("--split_names", + nargs="+", + default=["image_wise"], + help="dataset splits to use") + parser.add_argument("--output_dir", + type=str, + default="models", + help="path to store search data") + parser.add_argument("--search_name", + type=str, + default=None, + help="name of search") + parser.add_argument("--cpu_cores", + nargs="+", + default=[], + help="CPU cores to use") + parser.add_argument("--gpu_devices", + nargs="+", + default=[], + help="GPU devices to use") args = parser.parse_args() datasets = args.datasets base_model_dirs = args.base_model_dirs @@ -62,23 +88,42 @@ split_names = args.split_names output_dir = args.output_dir search_name = args.search_name - cpu_cores =[int(core) for core in args.cpu_cores] + cpu_cores = [int(core) for core in args.cpu_cores] gpu_devices = [int(device) for device in args.gpu_devices] - assert len(datasets) == len(train_configs), "Must have same number of datasets as training configs!" + assert len(datasets) == len( + train_configs + ), "Must have same number of datasets as training configs!" if len(base_model_dirs) > 0: - assert len(base_model_dirs) == len(datasets), "Must have same number of base models for fine-tuning as datasets and training configs!" + models_datasets_mismatch_msg = ("Must have same number of base models" + " for fine-tuning as datasets and" + " training configs!") + assert len(base_model_dirs) == len( + datasets), models_datasets_mismatch_msg if len(split_names) < len(datasets): if len(split_names) == 1: - logger.warning("Using split "{}" for all datasets/configs...".format(split_names[0])) + logger.warning( + "Using split \"{}\" for all datasets/configs...".format( + split_names[0])) split_names *= len(datasets) else: - raise ValueError("Can't have fewer splits that datasets/configs provided unless there is only one.") + not_enough_splits_msg = ("Can't have fewer splits that" + " datasets/configs provided unless there" + " is only one.") + raise ValueError(not_enough_splits_msg) # Parse configs. analysis_config = YamlConfig(analysis_config) train_configs = [YamlConfig(cfg) for cfg in train_configs] # Search. - search = GQCNNSearch(analysis_config, train_configs, datasets, split_names, output_dir=output_dir, search_name=search_name, cpu_cores=cpu_cores, gpu_devices=gpu_devices, base_models=base_model_dirs) + search = GQCNNSearch(analysis_config, + train_configs, + datasets, + split_names, + output_dir=output_dir, + search_name=search_name, + cpu_cores=cpu_cores, + gpu_devices=gpu_devices, + base_models=base_model_dirs) search.search() diff --git a/tools/plot_training_losses.py b/tools/plot_training_losses.py index b7b41a65..f9ee5e2d 100644 --- a/tools/plot_training_losses.py +++ b/tools/plot_training_losses.py @@ -32,7 +32,7 @@ ------------------- model_dir : str Command line argument, the path to the model whose errors are to plotted. - All plots and other metrics will be saved to this directory. + All plots and other metrics will be saved to this directory. """ from __future__ import absolute_import from __future__ import division @@ -52,7 +52,8 @@ if __name__ == "__main__": result_dir = sys.argv[1] - train_errors_filename = os.path.join(result_dir, GQCNNFilenames.TRAIN_ERRORS) + train_errors_filename = os.path.join(result_dir, + GQCNNFilenames.TRAIN_ERRORS) val_errors_filename = os.path.join(result_dir, GQCNNFilenames.VAL_ERRORS) train_iters_filename = os.path.join(result_dir, GQCNNFilenames.TRAIN_ITERS) val_iters_filename = os.path.join(result_dir, GQCNNFilenames.VAL_ITERS) @@ -71,37 +72,39 @@ val_losses = None try: val_losses = np.load(val_losses_filename) - except: + except FileNotFoundError: pass - + val_errors = np.r_[pct_pos_val, val_errors] val_iters = np.r_[0, val_iters] - + # Window the training error. i = 0 train_errors = [] train_losses = [] train_iters = [] while i < raw_train_errors.shape[0]: - train_errors.append(np.mean(raw_train_errors[i:i+WINDOW])) - train_losses.append(np.mean(raw_train_losses[i:i+WINDOW])) + train_errors.append( + np.mean(raw_train_errors[i:i + GeneralConstants.WINDOW])) + train_losses.append( + np.mean(raw_train_losses[i:i + GeneralConstants.WINDOW])) train_iters.append(i) - i += WINDOW + i += GeneralConstants.WINDOW train_errors = np.array(train_errors) train_losses = np.array(train_losses) train_iters = np.array(train_iters) if val_losses is not None: val_losses = np.r_[train_losses[0], val_losses] - + init_val_error = val_errors[0] norm_train_errors = train_errors / init_val_error norm_val_errors = val_errors / init_val_error norm_final_val_error = val_errors[-1] / val_errors[0] if pct_pos_val > 0: - norm_final_val_error = val_errors[-1] / pct_pos_val + norm_final_val_error = val_errors[-1] / pct_pos_val - logger.info("TRAIN") + logger.info("TRAIN") logger.info("Original Error {}".format(train_errors[0])) logger.info("Final Error {}".format(train_errors[-1])) logger.info("Orig loss {}".format(train_losses[0])) @@ -122,7 +125,7 @@ plt.legend(("Training (Minibatch)", "Validation"), fontsize=15, loc="best") plt.xlabel("Iteration", fontsize=15) plt.ylabel("Error Rate", fontsize=15) - + plt.figure() plt.plot(train_iters, norm_train_errors, linewidth=4, color="b") plt.plot(val_iters, norm_val_errors, linewidth=4, color="g") @@ -146,8 +149,8 @@ plt.xlabel("Iteration", fontsize=15) plt.ylabel("Validation Loss", fontsize=15) plt.show() - - plt.figure(figsize=(8,6)) + + plt.figure(figsize=(8, 6)) plt.plot(train_iters, train_errors, linewidth=4, color="b") plt.plot(val_iters, val_errors, linewidth=4, color="g") plt.ylim(0, 100) @@ -156,7 +159,7 @@ plt.ylabel("Error Rate", fontsize=15) plt.savefig(os.path.join(result_dir, "training_curve.jpg")) - plt.figure(figsize=(8,6)) + plt.figure(figsize=(8, 6)) plt.plot(train_iters, norm_train_errors, linewidth=4, color="b") plt.plot(val_iters, norm_val_errors, linewidth=4, color="g") plt.ylim(0, 2.0) diff --git a/tools/run_policy.py b/tools/run_policy.py index c382bed1..4abc5990 100644 --- a/tools/run_policy.py +++ b/tools/run_policy.py @@ -23,6 +23,7 @@ MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. Script to run saved policy output from user. + Author ------ Jeff Mahler @@ -38,7 +39,7 @@ import numpy as np -from autolab_core import RigidTransform, YamlConfig, Logger +from autolab_core import YamlConfig, Logger from visualization import Visualizer2D as vis2d from gqcnn import RgbdImageState, ParallelJawGrasp from gqcnn import CrossEntropyRobustGraspingPolicy @@ -48,10 +49,21 @@ if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Run a saved test case through a GQ-CNN policy. For debugging purposes only.") - parser.add_argument("test_case_path", type=str, default=None, help="path to test case") - parser.add_argument("--config_filename", type=str, default="cfg/tools/run_policy.yaml", help="path to configuration file to use") - parser.add_argument("--output_dir", type=str, default=None, help="directory to store output") + parser = argparse.ArgumentParser( + description=("Run a saved test case through a GQ-CNN policy. For" + " debugging purposes only.")) + parser.add_argument("test_case_path", + type=str, + default=None, + help="path to test case") + parser.add_argument("--config_filename", + type=str, + default="cfg/tools/run_policy.yaml", + help="path to configuration file to use") + parser.add_argument("--output_dir", + type=str, + default=None, + help="directory to store output") parser.add_argument("--seed", type=int, default=None, help="random seed") args = parser.parse_args() test_case_path = args.test_case_path @@ -62,22 +74,21 @@ # Make output dir. if output_dir is not None and not os.path.exists(output_dir): os.mkdir(output_dir) - + # Make relative paths absolute. if not os.path.isabs(config_filename): - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - config_filename) + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", config_filename) # Set random seed. if seed is not None: np.random.seed(seed) random.seed(seed) - + # Read config. config = YamlConfig(config_filename) policy_config = config["policy"] - + # Load test case. state_path = os.path.join(test_case_path, "state") action_path = os.path.join(test_case_path, "action") @@ -90,53 +101,61 @@ if policy_config["vis"]["input_images"]: vis2d.figure() if state.segmask is None: - vis2d.subplot(1,2,1) + vis2d.subplot(1, 2, 1) vis2d.imshow(state.rgbd_im.color) vis2d.title("COLOR") - vis2d.subplot(1,2,2) + vis2d.subplot(1, 2, 2) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config["vis"]["vmin"], - vmax=policy_config["vis"]["vmax"]) + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) vis2d.title("DEPTH") else: - vis2d.subplot(1,3,1) + vis2d.subplot(1, 3, 1) vis2d.imshow(state.rgbd_im.color) vis2d.title("COLOR") - vis2d.subplot(1,3,2) + vis2d.subplot(1, 3, 2) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config["vis"]["vmin"], - vmax=policy_config["vis"]["vmax"]) + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) vis2d.title("DEPTH") - vis2d.subplot(1,3,3) - vis2d.imshow(state.segmask) + vis2d.subplot(1, 3, 3) + vis2d.imshow(state.segmask) vis2d.title("SEGMASK") filename = None if output_dir is not None: filename = os.path.join(output_dir, "input_images.png") - vis2d.show(filename) + vis2d.show(filename) # Query policy. policy_start = time.time() action = policy(state) - logger.info("Planning took %.3f sec" %(time.time() - policy_start)) + logger.info("Planning took %.3f sec" % (time.time() - policy_start)) # Vis final grasp. if policy_config["vis"]["final_grasp"]: - vis2d.figure(size=(10,10)) - vis2d.subplot(1,2,1) + vis2d.figure(size=(10, 10)) + vis2d.subplot(1, 2, 1) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config["vis"]["vmin"], - vmax=policy_config["vis"]["vmax"]) - vis2d.grasp(original_action.grasp, scale=policy_config["vis"]["grasp_scale"], show_center=False, show_axis=True, color="r") - vis2d.title("Original (Q=%.3f) (Z=%.3f)" %(original_action.q_value, - original_action.grasp.depth)) - vis2d.subplot(1,2,2) + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) + vis2d.grasp(original_action.grasp, + scale=policy_config["vis"]["grasp_scale"], + show_center=False, + show_axis=True, + color="r") + vis2d.title("Original (Q=%.3f) (Z=%.3f)" % + (original_action.q_value, original_action.grasp.depth)) + vis2d.subplot(1, 2, 2) vis2d.imshow(state.rgbd_im.depth, - vmin=policy_config["vis"]["vmin"], - vmax=policy_config["vis"]["vmax"]) - vis2d.grasp(action.grasp, scale=policy_config["vis"]["grasp_scale"], show_center=False, show_axis=True, color="r") - vis2d.title("New (Q=%.3f) (Z=%.3f)" %(action.q_value, - action.grasp.depth)) + vmin=policy_config["vis"]["vmin"], + vmax=policy_config["vis"]["vmax"]) + vis2d.grasp(action.grasp, + scale=policy_config["vis"]["grasp_scale"], + show_center=False, + show_axis=True, + color="r") + vis2d.title("New (Q=%.3f) (Z=%.3f)" % + (action.q_value, action.grasp.depth)) filename = None if output_dir is not None: filename = os.path.join(output_dir, "planned_grasp.png") diff --git a/tools/train.py b/tools/train.py index 90e33824..15950e94 100644 --- a/tools/train.py +++ b/tools/train.py @@ -45,24 +45,47 @@ if __name__ == "__main__": # Parse args. - parser = argparse.ArgumentParser(description="Train a Grasp Quality Convolutional Neural Network with TensorFlow") - parser.add_argument("dataset_dir", type=str, default=None, - help="path to the dataset to use for training and validation") - parser.add_argument("--split_name", type=str, default="image_wise", + parser = argparse.ArgumentParser( + description=("Train a Grasp Quality Convolutional Neural Network with" + " TensorFlow")) + parser.add_argument( + "dataset_dir", + type=str, + default=None, + help="path to the dataset to use for training and validation") + parser.add_argument("--split_name", + type=str, + default="image_wise", help="name of the split to train on") - parser.add_argument("--output_dir", type=str, default=None, + parser.add_argument("--output_dir", + type=str, + default=None, help="path to store the model") - parser.add_argument("--tensorboard_port", type=int, default=None, + parser.add_argument("--tensorboard_port", + type=int, + default=None, help="port to launch tensorboard on") - parser.add_argument("--seed", type=int, default=None, + parser.add_argument("--seed", + type=int, + default=None, help="random seed for training") - parser.add_argument("--config_filename", type=str, default=None, + parser.add_argument("--config_filename", + type=str, + default=None, help="path to the configuration file to use") - parser.add_argument("--name", type=str, default=None, + parser.add_argument("--name", + type=str, + default=None, help="name for the trained model") - parser.add_argument("--save_datetime", type=bool, default=False, - help="whether or not to save a model with the date and time of training") - parser.add_argument("--backend", type=str, default="tf", + parser.add_argument( + "--save_datetime", + type=bool, + default=False, + help=("whether or not to save a model with the date and time of" + " training")) + parser.add_argument("--backend", + type=str, + default="tf", help="the deep learning framework to use") args = parser.parse_args() dataset_dir = args.dataset_dir @@ -74,17 +97,17 @@ name = args.name save_datetime = args.save_datetime backend = args.backend - + # Set default output dir. if output_dir is None: output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../models") - + # Set default config filename. if config_filename is None: - config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)), - "..", - "cfg/train.yaml") + config_filename = os.path.join( + os.path.dirname(os.path.realpath(__file__)), "..", + "cfg/train.yaml") # Turn relative paths absolute. if not os.path.isabs(dataset_dir): @@ -96,7 +119,7 @@ # Create output dir if necessary. utils.mkdir_safe(output_dir) - + # Open train config. train_config = YamlConfig(config_filename) if seed is not None: @@ -121,10 +144,11 @@ start_time = time.time() gqcnn = get_gqcnn_model(backend)(gqcnn_params) trainer = get_gqcnn_trainer(backend)(gqcnn, - dataset_dir, - split_name, - output_dir, - train_config, - name=name) + dataset_dir, + split_name, + output_dir, + train_config, + name=name) trainer.train() - logger.info("Total Training Time: " + str(utils.get_elapsed_time(time.time() - start_time))) + logger.info("Total Training Time: " + + str(utils.get_elapsed_time(time.time() - start_time))) From 83691862c8fadb9a7e061ca312f1ce43c448a016 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 11:41:03 -0700 Subject: [PATCH 05/29] Second round of YAPF. --- ci/travis/format.sh | 2 +- gqcnn/grasping/grasp.py | 6 +++--- gqcnn/model/tf/fc_network_tf.py | 2 +- gqcnn/model/tf/network_tf.py | 8 +++----- gqcnn/training/tf/trainer_tf.py | 33 +++++++++++++-------------------- 5 files changed, 21 insertions(+), 30 deletions(-) diff --git a/ci/travis/format.sh b/ci/travis/format.sh index f8fe53fb..a2291bac 100755 --- a/ci/travis/format.sh +++ b/ci/travis/format.sh @@ -17,7 +17,7 @@ format() { # Format all files, and print the diff to `stdout` for Travis. format_all() { - yapf --diff "${YAPF_FLAGS[@]}" "${YAPF_EXCLUDES[@]}" + yapf --diff "${YAPF_FLAGS[@]}" "${YAPF_EXCLUDES[@]}" . } # This flag formats individual files. `--files` *must* be the first command line diff --git a/gqcnn/grasping/grasp.py b/gqcnn/grasping/grasp.py index 254ecf61..d4b430ba 100644 --- a/gqcnn/grasping/grasp.py +++ b/gqcnn/grasping/grasp.py @@ -479,9 +479,9 @@ def __init__(self, pose, camera_intr=None): self._pose = pose # TODO(vsatish): Confirm that this is really not needed. -# frame = "image" -# if camera_intr is not None: -# frame = camera_intr.frame + # frame = "image" + # if camera_intr is not None: + # frame = camera_intr.frame # If `camera_intr` is `None` use default primesense camera intrinsics. if not camera_intr: diff --git a/gqcnn/model/tf/fc_network_tf.py b/gqcnn/model/tf/fc_network_tf.py index d48bef13..ef7dee87 100644 --- a/gqcnn/model/tf/fc_network_tf.py +++ b/gqcnn/model/tf/fc_network_tf.py @@ -80,7 +80,7 @@ def __init__(self, gqcnn_config, fc_config, verbose=True, log_file=None): " padding for conv layers. Found layer: {}" " with padding: {}") assert layer_config["pad"] == "VALID", invalid_pad_msg.format( - layer_name, layer_config["pad"]) + layer_name, layer_config["pad"]) @staticmethod def load(model_dir, fc_config, log_file=None): diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index a343e929..c85383bb 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -1416,11 +1416,9 @@ def _build_network(self, input_im_node, input_pose_node, self._input_depth_mode == InputDepthMode.IM_ONLY: extraneous_stream_msg = ("When using input depth mode \"{}\", only" " im stream is allowed!") - assert not ( - "pose_stream" in self._architecture - or "merge_stream" in self._architecture - ), extraneous_stream_msg.format( - self._input_depth_mode) + assert not ("pose_stream" in self._architecture + or "merge_stream" in self._architecture + ), extraneous_stream_msg.format(self._input_depth_mode) with tf.name_scope("im_stream"): return self._build_im_stream(input_im_node, input_pose_node, diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index 55b24bf8..1e6ed870 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -790,9 +790,8 @@ def _compute_data_metrics(self): num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info( - "Adding file {} of {}..." - .format(k + 1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {}...".format( + k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr depth_data = read_pose_data( self.dataset.tensor(self.pose_field_name, i).arr, @@ -812,9 +811,8 @@ def _compute_data_metrics(self): self.logger.info("Computing (image - depth) std.") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info( - "Adding file {} of {}..." - .format(k + 1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {}...".format( + k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr depth_data = read_pose_data( self.dataset.tensor(self.pose_field_name, i).arr, @@ -857,9 +855,8 @@ def _compute_data_metrics(self): num_summed = 0 for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info( - "Adding file {} of {}..." - .format(k + 1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {}...".format( + k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: @@ -872,9 +869,8 @@ def _compute_data_metrics(self): self.logger.info("Computing image std.") for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info( - "Adding file {} of {}..." - .format(k + 1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {}...".format( + k + 1, random_file_indices.shape[0])) im_data = self.dataset.tensor(self.im_field_name, i).arr train_indices = self.train_index_map[i] if train_indices.shape[0] > 0: @@ -907,9 +903,8 @@ def _compute_data_metrics(self): # Read metrics. for k, i in enumerate(random_file_indices): if k % self.preproc_log_frequency == 0: - self.logger.info( - "Adding file {} of {}...".format( - k + 1, random_file_indices.shape[0])) + self.logger.info("Adding file {} of {}...".format( + k + 1, random_file_indices.shape[0])) metric_data = self.dataset.tensor(self.label_field_name, i).arr train_indices = self.train_index_map[i] val_indices = self.val_index_map[i] @@ -1042,9 +1037,7 @@ def _save_configs(self): for key in self.cfg: tempOrderedDict[key] = self.cfg[key] with open(out_config_filename, "w") as outfile: - json.dump(tempOrderedDict, - outfile, - indent=JSON_INDENT) + json.dump(tempOrderedDict, outfile, indent=JSON_INDENT) # Save training script. this_filename = sys.argv[0] @@ -1190,8 +1183,8 @@ def _open_dataset(self): # Read split. if not self.dataset.has_split(self.split_name): self.logger.info( - "Dataset split: {} not found. Creating new split..." - .format(self.split_name)) + "Dataset split: {} not found. Creating new split...".format( + self.split_name)) self.dataset.make_split(self.split_name, train_pct=self.train_pct) else: self.logger.info("Training split: {} found in dataset.".format( From 42bcc33997a857c8bef8734f48b3c100c7ed60cd Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 13:11:22 -0700 Subject: [PATCH 06/29] Added Travis CI. --- .travis.yaml | 25 +++++++++++++++++++++++++ setup.py | 5 ++++- 2 files changed, 29 insertions(+), 1 deletion(-) create mode 100644 .travis.yaml diff --git a/.travis.yaml b/.travis.yaml new file mode 100644 index 00000000..e762a2c7 --- /dev/null +++ b/.travis.yaml @@ -0,0 +1,25 @@ +language: python + +matrix: + include: + - name: "Python 2.7 on Trusty Linux" + python: 2.7 + dist: trusty + + - name: "Python 3.5 on Xenial Linux" + python: 3.5 + dist: xenial + + - name: "Python 3.6 on Xenial Linux" + python: 3.6 + dist: xenial + + - name: "Python 3.7 on Xenial Linux" + python: 3.7 + dist: xenial + +install: + - pip install . + +script: + - python --version && python -c "import gqcnn" diff --git a/setup.py b/setup.py index e6535924..47b7904c 100644 --- a/setup.py +++ b/setup.py @@ -166,7 +166,10 @@ def run(self): classifiers=[ "Development Status :: 4 - Beta", "Programming Language :: Python :: 2.7", - "Programming Language :: Python :: 3", "Natural Language :: English", + "Programming Language :: Python :: 3.5", + "Programming Language :: Python :: 3.6", + "Programming Language :: Python :: 3.7", + "Natural Language :: English", "Topic :: Scientific/Engineering" ], packages=find_packages(), From de3c9f6de46bee94127f939b2744488db5eedb1e Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 14:21:02 -0700 Subject: [PATCH 07/29] Fixed Travis YAML config file extension. --- .travis.yaml => .travis.yml | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename .travis.yaml => .travis.yml (100%) diff --git a/.travis.yaml b/.travis.yml similarity index 100% rename from .travis.yaml rename to .travis.yml From 87f6af2f6af7a47251c2b067603774b5732420f3 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 14:36:31 -0700 Subject: [PATCH 08/29] WIP fixing missing GLU. --- .travis.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.travis.yml b/.travis.yml index e762a2c7..bbddaac5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,6 +18,10 @@ matrix: python: 3.7 dist: xenial +before_install: + - sudo apt-get update + - sudo apt-get install python-opengl + install: - pip install . From 521ac353bd1b36ca466abb22447f79f6bff12a5a Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 15:24:28 -0700 Subject: [PATCH 09/29] Added linting to build. --- .travis.yml | 18 ++++++++++++++++-- setup.py | 8 ++++---- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index bbddaac5..a7e8b540 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,12 +18,26 @@ matrix: python: 3.7 dist: xenial + - name: "Linter" + python: 3.7 + dist: xenial + before_install: [] + install: [] + script: + - pip install yapf==0.27.0 # Must use specific version. + - pip install flake8 flake8-comprehensions flake8-quotes==2.0.0 + - ./ci/travis/format.sh --all # Should exit with 0 for no diffs. + - flake8 --inline-quotes '"' --no-avoid-escape --exclude=docs/source/conf.py --ignore=C408,E121,E123,E126,E226,E24,E704,W503,W504,W605 + before_install: - sudo apt-get update - - sudo apt-get install python-opengl + - sudo apt-get install python-opengl # For GLU. install: - pip install . script: - - python --version && python -c "import gqcnn" + - python -c "import gqcnn" + +notifications: + email: false diff --git a/setup.py b/setup.py index 47b7904c..0cd47178 100644 --- a/setup.py +++ b/setup.py @@ -166,10 +166,10 @@ def run(self): classifiers=[ "Development Status :: 4 - Beta", "Programming Language :: Python :: 2.7", - "Programming Language :: Python :: 3.5", - "Programming Language :: Python :: 3.6", - "Programming Language :: Python :: 3.7", - "Natural Language :: English", + "Programming Language :: Python :: 3.5", + "Programming Language :: Python :: 3.6", + "Programming Language :: Python :: 3.7", + "Natural Language :: English", # yapf: disable "Topic :: Scientific/Engineering" ], packages=find_packages(), From ac08d939a256dac02e32c30212e5ddc8337b81d8 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 19:14:10 -0700 Subject: [PATCH 10/29] Removed use of sort(). --- gqcnn/grasping/policy/policy.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gqcnn/grasping/policy/policy.py b/gqcnn/grasping/policy/policy.py index ec5855e8..2dd6a557 100644 --- a/gqcnn/grasping/policy/policy.py +++ b/gqcnn/grasping/policy/policy.py @@ -795,7 +795,7 @@ def select(self, grasps, q_values): if num_grasps == 0: raise NoValidGraspsException("Zero grasps") grasps_and_predictions = zip(np.arange(num_grasps), q_values) - grasps_and_predictions.sort(key=lambda x: x[1], reverse=True) + grasps_and_predictions = sorted(grasps_and_predictions, key=lambda x: x[1], reverse=True) # Return top grasps. if self._filters is None: @@ -1039,7 +1039,7 @@ def action_set(self, state): # Sort grasps. resample_start = time() q_values_and_indices = zip(q_values, np.arange(num_grasps)) - q_values_and_indices.sort(key=lambda x: x[0], reverse=True) + q_values_and_indices = sorted(q_values_and_indices, key=lambda x: x[0], reverse=True) if self.config["vis"]["grasp_candidates"]: # Display each grasp on the original image, colored by From c02c8b41e1ec940a36adad370e70ba1c54177f00 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Sun, 26 May 2019 21:55:00 -0700 Subject: [PATCH 11/29] Updating cosmetic Python 3 stuff. --- README.md | 4 +++- docs/source/install/install.rst | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index bbf6c8e3..6efd179d 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,10 @@ # Berkeley AUTOLAB's GQCNN Package

+ Build Status Release Software License - Python Version + Python 2 Version + Python 2 Version

## Package Overview diff --git a/docs/source/install/install.rst b/docs/source/install/install.rst index 8c675def..566db663 100644 --- a/docs/source/install/install.rst +++ b/docs/source/install/install.rst @@ -4,7 +4,7 @@ Prerequisites Python """""" -The `gqcnn` package has only been tested with `Python 2.7`. +The `gqcnn` package has only been tested with `Python 2.7`, `Python 3.5`, `Python 3.6`, and `Python 3.7`. Ubuntu """""" From 85c59e01b1661f07476dda1e618e576d64541a62 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Mon, 27 May 2019 09:38:09 -0700 Subject: [PATCH 12/29] Removing all uses of sort(). --- gqcnn/grasping/policy/fc_policy.py | 2 +- gqcnn/grasping/policy/policy.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gqcnn/grasping/policy/fc_policy.py b/gqcnn/grasping/policy/fc_policy.py index 4dc848a5..bc64d262 100644 --- a/gqcnn/grasping/policy/fc_policy.py +++ b/gqcnn/grasping/policy/fc_policy.py @@ -303,7 +303,7 @@ def _action(self, state, num_actions=1): # Filter grasps. if self._filter_grasps: - actions.sort(reverse=True, key=lambda action: action.q_value) + actions = sorted(actions, reverse=True, key=lambda action: action.q_value) actions = [self._filter(actions)] # Visualize. diff --git a/gqcnn/grasping/policy/policy.py b/gqcnn/grasping/policy/policy.py index 2dd6a557..5336d564 100644 --- a/gqcnn/grasping/policy/policy.py +++ b/gqcnn/grasping/policy/policy.py @@ -565,7 +565,7 @@ def select(self, grasps, q_value): # Sort grasps. num_grasps = len(grasps) grasps_and_predictions = zip(np.arange(num_grasps), q_value) - grasps_and_predictions.sort(key=lambda x: x[1], reverse=True) + grasps_and_predictions = sorted(grasps_and_predictions, key=lambda x: x[1], reverse=True) # Return top grasps. if self._filters is None: @@ -1059,7 +1059,7 @@ def action_set(self, state): save_fname="cem_iter_{}.png".format(j), save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) - display_grasps_and_q_values.sort(key=lambda x: x[1]) + display_grasps_and_q_values = sorted(display_grasps_and_q_values, key=lambda x: x[1]) vis.figure(size=(GeneralConstants.FIGSIZE, GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, @@ -1241,7 +1241,7 @@ def action_set(self, state): save_fname="final_sampled_grasps.png".format(j), save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) - display_grasps_and_q_values.sort(key=lambda x: x[1]) + display_grasps_and_q_values = sorted(display_grasps_and_q_values, key=lambda x: x[1]) vis.figure(size=(GeneralConstants.FIGSIZE, GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, @@ -1633,7 +1633,7 @@ def action(self, state, policy_subset=None, min_q_value=-1.0): raise NoValidGraspsException() # Rank based on q value. - actions.sort(key=lambda x: x.q_value, reverse=True) + actions = sorted(actions, key=lambda x: x.q_value, reverse=True) return actions[0] def action_set(self, state, policy_subset=None, min_q_value=-1.0): From 2ea8ad593471a375e63a2a80802eb34085aba7c6 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Mon, 27 May 2019 09:52:35 -0700 Subject: [PATCH 13/29] Updating TF and Scikit-Image versions. --- requirements/cpu_requirements.txt | 4 ++-- requirements/gpu_requirements.txt | 4 ++-- setup.py | 7 +++---- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/requirements/cpu_requirements.txt b/requirements/cpu_requirements.txt index e6886666..e5ead1ca 100644 --- a/requirements/cpu_requirements.txt +++ b/requirements/cpu_requirements.txt @@ -5,8 +5,8 @@ numpy opencv-python scipy matplotlib -tensorflow>=1.10.0,<=1.13.1 +tensorflow<=1.13.1 scikit-learn -scikit-image +scikit-image<=0.14.2 gputil psutil diff --git a/requirements/gpu_requirements.txt b/requirements/gpu_requirements.txt index e8b37f09..3430d188 100644 --- a/requirements/gpu_requirements.txt +++ b/requirements/gpu_requirements.txt @@ -5,8 +5,8 @@ numpy opencv-python scipy matplotlib -tensorflow-gpu>=1.10.0,<=1.13.1 +tensorflow-gpu<=1.13.1 scikit-learn -scikit-image +scikit-image<=0.14.2 gputil psutil diff --git a/setup.py b/setup.py index 0cd47178..33ebe366 100644 --- a/setup.py +++ b/setup.py @@ -40,7 +40,6 @@ import subprocess import sys -TF_MIN_VERSION = "1.10.0" TF_MAX_VERSION = "1.13.1" # Set up logger. @@ -52,13 +51,13 @@ def get_tf_dep(): # Check whether or not the Nvidia driver and GPUs are available and add the # corresponding Tensorflow dependency. - tf_dep = "tensorflow>={},<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) + tf_dep = "tensorflow<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) try: gpus = subprocess.check_output( ["nvidia-smi", "--query-gpu=gpu_name", "--format=csv"]).decode().strip().split("\n")[1:] if len(gpus) > 0: - tf_dep = "tensorflow-gpu>={},<={}".format(TF_MIN_VERSION, + tf_dep = "tensorflow-gpu<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) else: no_device_msg = ("Found Nvidia device driver but no" @@ -144,7 +143,7 @@ def run(self): requirements = [ "autolab-core", "autolab-perception", "visualization", "numpy", "scipy", - "matplotlib", "opencv-python", "scikit-image", "scikit-learn", "psutil", + "matplotlib", "opencv-python", "scikit-image<=0.14.2", "scikit-learn", "psutil", "gputil" ] From 81b298c6a13e052673eb4480751bec1c43cea745 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Mon, 27 May 2019 21:29:42 -0700 Subject: [PATCH 14/29] Added license to all files where it is necessary. --- .travis.yml | 21 ++++++++++++++ CMakeLists.txt | 22 ++++++++++++++ cfg/examples/antipodal_grasp_sampling.yaml | 22 ++++++++++++++ cfg/examples/fc_gqcnn_pj.yaml | 22 ++++++++++++++ cfg/examples/fc_gqcnn_suction.yaml | 22 ++++++++++++++ cfg/examples/gqcnn_pj.yaml | 22 ++++++++++++++ cfg/examples/gqcnn_suction.yaml | 22 ++++++++++++++ cfg/examples/replication/dex-net_2.0.yaml | 22 ++++++++++++++ cfg/examples/replication/dex-net_2.1.yaml | 22 ++++++++++++++ cfg/examples/replication/dex-net_3.0.yaml | 22 ++++++++++++++ .../replication/dex-net_4.0_fc_pj.yaml | 22 ++++++++++++++ .../replication/dex-net_4.0_fc_suction.yaml | 22 ++++++++++++++ cfg/examples/replication/dex-net_4.0_pj.yaml | 22 ++++++++++++++ .../replication/dex-net_4.0_suction.yaml | 22 ++++++++++++++ cfg/examples/ros/fc_gqcnn_pj.yaml | 22 ++++++++++++++ cfg/examples/ros/fc_gqcnn_suction.yaml | 22 ++++++++++++++ cfg/examples/ros/gqcnn_pj.yaml | 22 ++++++++++++++ cfg/examples/ros/gqcnn_suction.yaml | 22 ++++++++++++++ cfg/finetune.yaml | 22 ++++++++++++++ cfg/finetune_dex-net_4.0_pj.yaml | 22 ++++++++++++++ cfg/finetune_dex-net_4.0_suction.yaml | 22 ++++++++++++++ cfg/finetune_example_pj.yaml | 22 ++++++++++++++ cfg/finetune_example_suction.yaml | 22 ++++++++++++++ ...-net_4.0_fc_suction_hyperparam_search.yaml | 22 ++++++++++++++ .../train_hyperparam_search.yaml | 22 ++++++++++++++ cfg/tools/analyze_gqcnn_performance.yaml | 22 ++++++++++++++ cfg/tools/run_policy.yaml | 22 ++++++++++++++ cfg/train.yaml | 22 ++++++++++++++ cfg/train_dex-net_2.0.yaml | 22 ++++++++++++++ cfg/train_dex-net_3.0.yaml | 22 ++++++++++++++ cfg/train_dex-net_4.0_fc_pj.yaml | 22 ++++++++++++++ cfg/train_dex-net_4.0_fc_suction.yaml | 22 ++++++++++++++ cfg/train_dex-net_4.0_pj.yaml | 22 ++++++++++++++ cfg/train_dex-net_4.0_suction.yaml | 22 ++++++++++++++ cfg/train_example_pj.yaml | 22 ++++++++++++++ cfg/train_example_suction.yaml | 22 ++++++++++++++ cfg/train_fc.yaml | 22 ++++++++++++++ ci/travis/format.sh | 22 ++++++++++++++ gqcnn/model/tf/network_tf.py | 2 +- launch/grasp_planning_service.launch | 26 ++++++++++++++++- msg/Action.msg | 22 ++++++++++++++ msg/BoundingBox.msg | 22 ++++++++++++++ msg/GQCNNGrasp.msg | 22 ++++++++++++++ msg/Observation.msg | 22 ++++++++++++++ package.xml | 29 +++++++++++++++++-- scripts/docker/build-docker.sh | 23 +++++++++++++++ .../datasets/download_dex-net_2.0.sh | 22 ++++++++++++++ .../datasets/download_dex-net_2.1.sh | 22 ++++++++++++++ .../datasets/download_dex-net_3.0.sh | 22 ++++++++++++++ .../datasets/download_dex-net_4.0_fc_pj.sh | 22 ++++++++++++++ .../download_dex-net_4.0_fc_suction.sh | 22 ++++++++++++++ .../datasets/download_dex-net_4.0_pj.sh | 22 ++++++++++++++ .../datasets/download_dex-net_4.0_suction.sh | 22 ++++++++++++++ .../datasets/download_example_datasets.sh | 22 ++++++++++++++ scripts/downloads/download_example_data.sh | 22 ++++++++++++++ scripts/downloads/models/download_models.sh | 22 ++++++++++++++ .../policies/run_all_dex-net_2.0_examples.sh | 22 ++++++++++++++ .../policies/run_all_dex-net_2.1_examples.sh | 22 ++++++++++++++ .../policies/run_all_dex-net_3.0_examples.sh | 22 ++++++++++++++ .../run_all_dex-net_4.0_fc_pj_examples.sh | 22 ++++++++++++++ ...run_all_dex-net_4.0_fc_suction_examples.sh | 22 ++++++++++++++ .../run_all_dex-net_4.0_pj_examples.sh | 22 ++++++++++++++ .../run_all_dex-net_4.0_suction_examples.sh | 22 ++++++++++++++ scripts/training/train_dex-net_2.0.sh | 22 ++++++++++++++ scripts/training/train_dex-net_2.1.sh | 25 ++++++++++++++++ scripts/training/train_dex-net_3.0.sh | 22 ++++++++++++++ scripts/training/train_dex-net_4.0_fc_pj.sh | 25 ++++++++++++++++ .../training/train_dex-net_4.0_fc_suction.sh | 25 ++++++++++++++++ scripts/training/train_dex-net_4.0_pj.sh | 25 ++++++++++++++++ scripts/training/train_dex-net_4.0_suction.sh | 25 ++++++++++++++++ srv/GQCNNGraspPlanner.srv | 24 ++++++++++++++- srv/GQCNNGraspPlannerBoundingBox.srv | 24 ++++++++++++++- srv/GQCNNGraspPlannerFull.srv | 24 ++++++++++++++- srv/GQCNNGraspPlannerSegmask.srv | 24 ++++++++++++++- 74 files changed, 1633 insertions(+), 9 deletions(-) diff --git a/.travis.yml b/.travis.yml index a7e8b540..23313ef6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,3 +1,24 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. language: python matrix: diff --git a/CMakeLists.txt b/CMakeLists.txt index f9674d6e..3c3734c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + cmake_minimum_required(VERSION 2.8.3) project(gqcnn) diff --git a/cfg/examples/antipodal_grasp_sampling.yaml b/cfg/examples/antipodal_grasp_sampling.yaml index 26753833..58d7a2c3 100644 --- a/cfg/examples/antipodal_grasp_sampling.yaml +++ b/cfg/examples/antipodal_grasp_sampling.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # policy params policy: # general params diff --git a/cfg/examples/fc_gqcnn_pj.yaml b/cfg/examples/fc_gqcnn_pj.yaml index 44ab1226..39c027ec 100644 --- a/cfg/examples/fc_gqcnn_pj.yaml +++ b/cfg/examples/fc_gqcnn_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: type: fully_conv_pj diff --git a/cfg/examples/fc_gqcnn_suction.yaml b/cfg/examples/fc_gqcnn_suction.yaml index 2c773bbe..122ed7ff 100644 --- a/cfg/examples/fc_gqcnn_suction.yaml +++ b/cfg/examples/fc_gqcnn_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: type: fully_conv_suction diff --git a/cfg/examples/gqcnn_pj.yaml b/cfg/examples/gqcnn_pj.yaml index ca8b683a..fb3f8c79 100644 --- a/cfg/examples/gqcnn_pj.yaml +++ b/cfg/examples/gqcnn_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: # optimization params num_seed_samples: 128 diff --git a/cfg/examples/gqcnn_suction.yaml b/cfg/examples/gqcnn_suction.yaml index 54119996..0ad50ab5 100644 --- a/cfg/examples/gqcnn_suction.yaml +++ b/cfg/examples/gqcnn_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: # optimization params num_seed_samples: 200 diff --git a/cfg/examples/replication/dex-net_2.0.yaml b/cfg/examples/replication/dex-net_2.0.yaml index 8800105e..f2ba3ac9 100644 --- a/cfg/examples/replication/dex-net_2.0.yaml +++ b/cfg/examples/replication/dex-net_2.0.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # policy params policy: # optimization params diff --git a/cfg/examples/replication/dex-net_2.1.yaml b/cfg/examples/replication/dex-net_2.1.yaml index c9c79423..93391ea9 100644 --- a/cfg/examples/replication/dex-net_2.1.yaml +++ b/cfg/examples/replication/dex-net_2.1.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # policy params policy: # optimization params diff --git a/cfg/examples/replication/dex-net_3.0.yaml b/cfg/examples/replication/dex-net_3.0.yaml index 688694ad..e8b9bca2 100644 --- a/cfg/examples/replication/dex-net_3.0.yaml +++ b/cfg/examples/replication/dex-net_3.0.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # policy params policy: # optimization params diff --git a/cfg/examples/replication/dex-net_4.0_fc_pj.yaml b/cfg/examples/replication/dex-net_4.0_fc_pj.yaml index ef48a4d3..51b714ff 100644 --- a/cfg/examples/replication/dex-net_4.0_fc_pj.yaml +++ b/cfg/examples/replication/dex-net_4.0_fc_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: type: fully_conv_pj diff --git a/cfg/examples/replication/dex-net_4.0_fc_suction.yaml b/cfg/examples/replication/dex-net_4.0_fc_suction.yaml index 765ecbf6..881b3e08 100644 --- a/cfg/examples/replication/dex-net_4.0_fc_suction.yaml +++ b/cfg/examples/replication/dex-net_4.0_fc_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: type: fully_conv_suction diff --git a/cfg/examples/replication/dex-net_4.0_pj.yaml b/cfg/examples/replication/dex-net_4.0_pj.yaml index 1396e272..0e970c56 100644 --- a/cfg/examples/replication/dex-net_4.0_pj.yaml +++ b/cfg/examples/replication/dex-net_4.0_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: # optimization params num_seed_samples: 128 diff --git a/cfg/examples/replication/dex-net_4.0_suction.yaml b/cfg/examples/replication/dex-net_4.0_suction.yaml index 3084b5d8..1d8d5d9c 100644 --- a/cfg/examples/replication/dex-net_4.0_suction.yaml +++ b/cfg/examples/replication/dex-net_4.0_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + policy: # optimization params num_seed_samples: 200 diff --git a/cfg/examples/ros/fc_gqcnn_pj.yaml b/cfg/examples/ros/fc_gqcnn_pj.yaml index 5aa5c93a..6a58ccf1 100644 --- a/cfg/examples/ros/fc_gqcnn_pj.yaml +++ b/cfg/examples/ros/fc_gqcnn_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + !include ../fc_gqcnn_pj.yaml # ROS-specific visualization diff --git a/cfg/examples/ros/fc_gqcnn_suction.yaml b/cfg/examples/ros/fc_gqcnn_suction.yaml index dcb3f34e..ba840635 100644 --- a/cfg/examples/ros/fc_gqcnn_suction.yaml +++ b/cfg/examples/ros/fc_gqcnn_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + !include ../fc_gqcnn_suction.yaml # ROS-specific visualization diff --git a/cfg/examples/ros/gqcnn_pj.yaml b/cfg/examples/ros/gqcnn_pj.yaml index 0558cfac..c69407e9 100644 --- a/cfg/examples/ros/gqcnn_pj.yaml +++ b/cfg/examples/ros/gqcnn_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + !include ../gqcnn_pj.yaml # ROS-specific visualization diff --git a/cfg/examples/ros/gqcnn_suction.yaml b/cfg/examples/ros/gqcnn_suction.yaml index 934b329d..e29fe968 100644 --- a/cfg/examples/ros/gqcnn_suction.yaml +++ b/cfg/examples/ros/gqcnn_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + !include ../gqcnn_suction.yaml # ROS-specific visualization diff --git a/cfg/finetune.yaml b/cfg/finetune.yaml index 97967174..7b3d95e7 100644 --- a/cfg/finetune.yaml +++ b/cfg/finetune.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/finetune_dex-net_4.0_pj.yaml b/cfg/finetune_dex-net_4.0_pj.yaml index c70fdd32..696eee96 100644 --- a/cfg/finetune_dex-net_4.0_pj.yaml +++ b/cfg/finetune_dex-net_4.0_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/finetune_dex-net_4.0_suction.yaml b/cfg/finetune_dex-net_4.0_suction.yaml index aa2cacbc..dcc0ea07 100644 --- a/cfg/finetune_dex-net_4.0_suction.yaml +++ b/cfg/finetune_dex-net_4.0_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/finetune_example_pj.yaml b/cfg/finetune_example_pj.yaml index cc4d44f9..89aedc7f 100644 --- a/cfg/finetune_example_pj.yaml +++ b/cfg/finetune_example_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/finetune_example_suction.yaml b/cfg/finetune_example_suction.yaml index 8a6ab88b..d9ad73eb 100644 --- a/cfg/finetune_example_suction.yaml +++ b/cfg/finetune_example_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/hyperparam_search/train_dex-net_4.0_fc_suction_hyperparam_search.yaml b/cfg/hyperparam_search/train_dex-net_4.0_fc_suction_hyperparam_search.yaml index ed1a806b..e65f4a65 100644 --- a/cfg/hyperparam_search/train_dex-net_4.0_fc_suction_hyperparam_search.yaml +++ b/cfg/hyperparam_search/train_dex-net_4.0_fc_suction_hyperparam_search.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/hyperparam_search/train_hyperparam_search.yaml b/cfg/hyperparam_search/train_hyperparam_search.yaml index 068d824a..5fc08638 100644 --- a/cfg/hyperparam_search/train_hyperparam_search.yaml +++ b/cfg/hyperparam_search/train_hyperparam_search.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + ### Example hyper-parameter search config corresponding to cfg/train.yaml that grid-searches through number of training epochs, base learning rate, and conv1_1 filter sizes. ### # general optimization params diff --git a/cfg/tools/analyze_gqcnn_performance.yaml b/cfg/tools/analyze_gqcnn_performance.yaml index dfaadb08..26abbf00 100644 --- a/cfg/tools/analyze_gqcnn_performance.yaml +++ b/cfg/tools/analyze_gqcnn_performance.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + log_rate: 10 font_size: 15 line_width: 4 diff --git a/cfg/tools/run_policy.yaml b/cfg/tools/run_policy.yaml index cd5d548f..e68ae52d 100644 --- a/cfg/tools/run_policy.yaml +++ b/cfg/tools/run_policy.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # policy params policy: # optimization params diff --git a/cfg/train.yaml b/cfg/train.yaml index 569d7e04..894ab85c 100644 --- a/cfg/train.yaml +++ b/cfg/train.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_dex-net_2.0.yaml b/cfg/train_dex-net_2.0.yaml index 3261675b..da324237 100644 --- a/cfg/train_dex-net_2.0.yaml +++ b/cfg/train_dex-net_2.0.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_dex-net_3.0.yaml b/cfg/train_dex-net_3.0.yaml index 2e44a7ba..7c40fc2c 100644 --- a/cfg/train_dex-net_3.0.yaml +++ b/cfg/train_dex-net_3.0.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_dex-net_4.0_fc_pj.yaml b/cfg/train_dex-net_4.0_fc_pj.yaml index 1fafeb50..141a1f0b 100644 --- a/cfg/train_dex-net_4.0_fc_pj.yaml +++ b/cfg/train_dex-net_4.0_fc_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_dex-net_4.0_fc_suction.yaml b/cfg/train_dex-net_4.0_fc_suction.yaml index 69f0bb65..369f9526 100644 --- a/cfg/train_dex-net_4.0_fc_suction.yaml +++ b/cfg/train_dex-net_4.0_fc_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_dex-net_4.0_pj.yaml b/cfg/train_dex-net_4.0_pj.yaml index 3483aa0d..f73c2fcb 100644 --- a/cfg/train_dex-net_4.0_pj.yaml +++ b/cfg/train_dex-net_4.0_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_dex-net_4.0_suction.yaml b/cfg/train_dex-net_4.0_suction.yaml index 04bd9b46..83a5ad34 100644 --- a/cfg/train_dex-net_4.0_suction.yaml +++ b/cfg/train_dex-net_4.0_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_example_pj.yaml b/cfg/train_example_pj.yaml index f53e0f94..b7e27327 100644 --- a/cfg/train_example_pj.yaml +++ b/cfg/train_example_pj.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_example_suction.yaml b/cfg/train_example_suction.yaml index a4733de5..682432f6 100644 --- a/cfg/train_example_suction.yaml +++ b/cfg/train_example_suction.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/cfg/train_fc.yaml b/cfg/train_fc.yaml index 1fafeb50..141a1f0b 100644 --- a/cfg/train_fc.yaml +++ b/cfg/train_fc.yaml @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # general optimization params train_batch_size: 64 val_batch_size: &val_batch_size 64 diff --git a/ci/travis/format.sh b/ci/travis/format.sh index a2291bac..f9983110 100755 --- a/ci/travis/format.sh +++ b/ci/travis/format.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # Script for YAPF formatting. Adapted from https://github.com/ray-project/ray/blob/master/ci/travis/format.sh. YAPF_FLAGS=( diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index c85383bb..8074fec4 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -1147,7 +1147,7 @@ def _build_fc_layer(self, return fc, out_size - # TODO(vsatish): This really doesn"t need to it"s own layer type...it does + # TODO(vsatish): This really doesn't need to it's own layer type...it does # the same thing as `_build_fc_layer`. def _build_pc_layer(self, input_node, fan_in, out_size, name): self._logger.info( diff --git a/launch/grasp_planning_service.launch b/launch/grasp_planning_service.launch index 1c253c8a..4dac1a2d 100644 --- a/launch/grasp_planning_service.launch +++ b/launch/grasp_planning_service.launch @@ -1,3 +1,27 @@ + + @@ -10,4 +34,4 @@ - \ No newline at end of file + diff --git a/msg/Action.msg b/msg/Action.msg index cc2fc22a..e05cce32 100644 --- a/msg/Action.msg +++ b/msg/Action.msg @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + uint32 width uint32 height uint8[] mask_data diff --git a/msg/BoundingBox.msg b/msg/BoundingBox.msg index 226983f0..a3b089f3 100644 --- a/msg/BoundingBox.msg +++ b/msg/BoundingBox.msg @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + float64 minX float64 minY float64 maxX diff --git a/msg/GQCNNGrasp.msg b/msg/GQCNNGrasp.msg index 4cc9eab4..41a07ae5 100644 --- a/msg/GQCNNGrasp.msg +++ b/msg/GQCNNGrasp.msg @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + geometry_msgs/Pose pose float64 q_value diff --git a/msg/Observation.msg b/msg/Observation.msg index 25b443f1..e2c6ca4c 100644 --- a/msg/Observation.msg +++ b/msg/Observation.msg @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + uint32 width uint32 height float32[] image_data diff --git a/package.xml b/package.xml index 5e8c7158..7371bc3a 100644 --- a/package.xml +++ b/package.xml @@ -1,8 +1,31 @@ + gqcnn - 1.0.0 - The GQCNN package + 1.1.0 + ROS package for deploying Grasp Quality Convolutional Neural Networks (GQ-CNNs). @@ -13,7 +36,7 @@ - Apache v2.0 + Regents diff --git a/scripts/docker/build-docker.sh b/scripts/docker/build-docker.sh index ef29fa7a..242dcdce 100755 --- a/scripts/docker/build-docker.sh +++ b/scripts/docker/build-docker.sh @@ -1,4 +1,27 @@ #!/bin/bash + +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # Build the CPU and GPU docker images. git archive --format=tar -o docker/gqcnn.tar --prefix=gqcnn/ master diff --git a/scripts/downloads/datasets/download_dex-net_2.0.sh b/scripts/downloads/datasets/download_dex-net_2.0.sh index f01deaed..86664ba0 100755 --- a/scripts/downloads/datasets/download_dex-net_2.0.sh +++ b/scripts/downloads/datasets/download_dex-net_2.0.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + wget -O data/training/dexnet_2.zip https://berkeley.box.com/shared/static/15oid8m9q6n9cvr8og4vm37bwghjjslp.zip cd data/training diff --git a/scripts/downloads/datasets/download_dex-net_2.1.sh b/scripts/downloads/datasets/download_dex-net_2.1.sh index a1453a99..c9d702ef 100755 --- a/scripts/downloads/datasets/download_dex-net_2.1.sh +++ b/scripts/downloads/datasets/download_dex-net_2.1.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + wget -O data/training/dexnet_2.1.zip https://berkeley.box.com/shared/static/4g0g0lstl45hv5g5232f89aoeccjk32j.zip cd data/training diff --git a/scripts/downloads/datasets/download_dex-net_3.0.sh b/scripts/downloads/datasets/download_dex-net_3.0.sh index ed840c97..d6da230e 100755 --- a/scripts/downloads/datasets/download_dex-net_3.0.sh +++ b/scripts/downloads/datasets/download_dex-net_3.0.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + wget -O data/training/dexnet_3.tar.gz https://berkeley.box.com/shared/static/wd5s51f1n786i71t8dufckec0262za4f.gz cd data/training diff --git a/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh b/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh index f2e03856..49f7dac0 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_fc_pj.sh @@ -1,3 +1,25 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + echo "Dex-Net 4.0 FC-GQ-CNN PJ dataset not yet publicly available. Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for access." diff --git a/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh b/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh index f2e03856..49f7dac0 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_fc_suction.sh @@ -1,3 +1,25 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + echo "Dex-Net 4.0 FC-GQ-CNN PJ dataset not yet publicly available. Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for access." diff --git a/scripts/downloads/datasets/download_dex-net_4.0_pj.sh b/scripts/downloads/datasets/download_dex-net_4.0_pj.sh index 39558b2a..95a72e97 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_pj.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_pj.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + wget -O data/training/dexnet_4_pj_aa https://berkeley.box.com/shared/static/ybqo02q6471odc2k80pltjwj24dh1gkz.0_pj_aa wget -O data/training/dexnet_4_pj_ab https://berkeley.box.com/shared/static/3id22ohgprdiv02ne031dgue0oe1r264.0_pj_ab wget -O data/training/dexnet_4_pj_ac https://berkeley.box.com/shared/static/9p49ilrcgi3t50rst3txo92ocr25ng2u.0_pj_ac diff --git a/scripts/downloads/datasets/download_dex-net_4.0_suction.sh b/scripts/downloads/datasets/download_dex-net_4.0_suction.sh index 93ef8c20..5a57f256 100755 --- a/scripts/downloads/datasets/download_dex-net_4.0_suction.sh +++ b/scripts/downloads/datasets/download_dex-net_4.0_suction.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + wget -O data/training/dexnet_4_suction_aa https://berkeley.box.com/shared/static/ev8wc4xf6m1zf61wrud18qbr2y4f7wyn.0_suction_aa wget -O data/training/dexnet_4_suction_ab https://berkeley.box.com/shared/static/1dbkz11fnspxk8bg379lqo0931i4cmxx.0_suction_ab wget -O data/training/dexnet_4_suction_ac https://berkeley.box.com/shared/static/xmlhcx3tl40jq01wwepsg46pkbtge3sp.0_suction_ac diff --git a/scripts/downloads/datasets/download_example_datasets.sh b/scripts/downloads/datasets/download_example_datasets.sh index 38688cf8..21cb4585 100755 --- a/scripts/downloads/datasets/download_example_datasets.sh +++ b/scripts/downloads/datasets/download_example_datasets.sh @@ -1,6 +1,28 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. # PARALLEL JAW. + wget -O data/training/example_training_dataset_pj.zip https://berkeley.box.com/shared/static/wpo8jbushrdq0adwjdsampui2tu1w1xz.zip mkdir -p data/training diff --git a/scripts/downloads/download_example_data.sh b/scripts/downloads/download_example_data.sh index 543d9555..53b1d0d3 100755 --- a/scripts/downloads/download_example_data.sh +++ b/scripts/downloads/download_example_data.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # DOWNLOAD MODELS (if they don't exist already). mkdir -p models cd models diff --git a/scripts/downloads/models/download_models.sh b/scripts/downloads/models/download_models.sh index dbc6934f..5f3c8ba0 100755 --- a/scripts/downloads/models/download_models.sh +++ b/scripts/downloads/models/download_models.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + mkdir -p models # STANDARD. diff --git a/scripts/policies/run_all_dex-net_2.0_examples.sh b/scripts/policies/run_all_dex-net_2.0_examples.sh index 4e7d9aed..9ce39775 100755 --- a/scripts/policies/run_all_dex-net_2.0_examples.sh +++ b/scripts/policies/run_all_dex-net_2.0_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/policies/run_all_dex-net_2.1_examples.sh b/scripts/policies/run_all_dex-net_2.1_examples.sh index 2f8b195d..f8e737ee 100755 --- a/scripts/policies/run_all_dex-net_2.1_examples.sh +++ b/scripts/policies/run_all_dex-net_2.1_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/policies/run_all_dex-net_3.0_examples.sh b/scripts/policies/run_all_dex-net_3.0_examples.sh index 78f7f573..5f21d487 100755 --- a/scripts/policies/run_all_dex-net_3.0_examples.sh +++ b/scripts/policies/run_all_dex-net_3.0_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh b/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh index f7802591..de6c1083 100755 --- a/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh b/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh index 068c8f62..d7e2d383 100755 --- a/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_fc_suction_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/policies/run_all_dex-net_4.0_pj_examples.sh b/scripts/policies/run_all_dex-net_4.0_pj_examples.sh index 67d70e6d..f9800eda 100755 --- a/scripts/policies/run_all_dex-net_4.0_pj_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_pj_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/policies/run_all_dex-net_4.0_suction_examples.sh b/scripts/policies/run_all_dex-net_4.0_suction_examples.sh index c66accfc..7b9fe98e 100755 --- a/scripts/policies/run_all_dex-net_4.0_suction_examples.sh +++ b/scripts/policies/run_all_dex-net_4.0_suction_examples.sh @@ -1,5 +1,27 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + set -e echo "RUNNING EXAMPLE 1" diff --git a/scripts/training/train_dex-net_2.0.sh b/scripts/training/train_dex-net_2.0.sh index 10dcf496..4898acef 100644 --- a/scripts/training/train_dex-net_2.0.sh +++ b/scripts/training/train_dex-net_2.0.sh @@ -1,3 +1,25 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + python tools/train.py data/training/dex-net_2.0 --config_filename cfg/train_dex-net_2.0.yaml --name GQCNN-2.0 diff --git a/scripts/training/train_dex-net_2.1.sh b/scripts/training/train_dex-net_2.1.sh index e69de29b..ebf2f36a 100644 --- a/scripts/training/train_dex-net_2.1.sh +++ b/scripts/training/train_dex-net_2.1.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +echo "Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for instructions on training Dex-Net 2.1." diff --git a/scripts/training/train_dex-net_3.0.sh b/scripts/training/train_dex-net_3.0.sh index b12351f9..2906d4da 100644 --- a/scripts/training/train_dex-net_3.0.sh +++ b/scripts/training/train_dex-net_3.0.sh @@ -1,3 +1,25 @@ #!/bin/bash +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + python tools/train.py data/training/dex-net_3.0 --config_filename cfg/train_dex-net_3.0.yaml --name GQCNN-3.0 diff --git a/scripts/training/train_dex-net_4.0_fc_pj.sh b/scripts/training/train_dex-net_4.0_fc_pj.sh index e69de29b..c466a673 100644 --- a/scripts/training/train_dex-net_4.0_fc_pj.sh +++ b/scripts/training/train_dex-net_4.0_fc_pj.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +echo "Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for instructions on training Dex-Net 4.0 FC PJ." diff --git a/scripts/training/train_dex-net_4.0_fc_suction.sh b/scripts/training/train_dex-net_4.0_fc_suction.sh index e69de29b..b4ac474b 100644 --- a/scripts/training/train_dex-net_4.0_fc_suction.sh +++ b/scripts/training/train_dex-net_4.0_fc_suction.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +echo "Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for instructions on training Dex-Net 4.0 FC Suction." diff --git a/scripts/training/train_dex-net_4.0_pj.sh b/scripts/training/train_dex-net_4.0_pj.sh index e69de29b..64fe8d65 100644 --- a/scripts/training/train_dex-net_4.0_pj.sh +++ b/scripts/training/train_dex-net_4.0_pj.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +echo "Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for instructions on training Dex-Net 4.0 PJ." diff --git a/scripts/training/train_dex-net_4.0_suction.sh b/scripts/training/train_dex-net_4.0_suction.sh index e69de29b..7cf95f97 100644 --- a/scripts/training/train_dex-net_4.0_suction.sh +++ b/scripts/training/train_dex-net_4.0_suction.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + +echo "Please contact Vishal Satish (vsatish@berkeley.edu) or Jeffrey Mahler (jmahler@berkeley.edu) for instructions on training Dex-Net 4.0 Suction." diff --git a/srv/GQCNNGraspPlanner.srv b/srv/GQCNNGraspPlanner.srv index 3c2a0ef2..10302807 100644 --- a/srv/GQCNNGraspPlanner.srv +++ b/srv/GQCNNGraspPlanner.srv @@ -1,7 +1,29 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # request params sensor_msgs/Image color_image sensor_msgs/Image depth_image sensor_msgs/CameraInfo camera_info --- # response params -GQCNNGrasp grasp \ No newline at end of file +GQCNNGrasp grasp diff --git a/srv/GQCNNGraspPlannerBoundingBox.srv b/srv/GQCNNGraspPlannerBoundingBox.srv index 72f070fd..7fa850d2 100644 --- a/srv/GQCNNGraspPlannerBoundingBox.srv +++ b/srv/GQCNNGraspPlannerBoundingBox.srv @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # request params sensor_msgs/Image color_image sensor_msgs/Image depth_image @@ -5,4 +27,4 @@ sensor_msgs/CameraInfo camera_info BoundingBox bounding_box --- # response params -GQCNNGrasp grasp \ No newline at end of file +GQCNNGrasp grasp diff --git a/srv/GQCNNGraspPlannerFull.srv b/srv/GQCNNGraspPlannerFull.srv index 9552a69f..e88d51f2 100644 --- a/srv/GQCNNGraspPlannerFull.srv +++ b/srv/GQCNNGraspPlannerFull.srv @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # request params sensor_msgs/Image color_image sensor_msgs/Image depth_image @@ -6,4 +28,4 @@ BoundingBox bounding_box sensor_msgs/Image segmask --- # response params -GQCNNGrasp grasp \ No newline at end of file +GQCNNGrasp grasp diff --git a/srv/GQCNNGraspPlannerSegmask.srv b/srv/GQCNNGraspPlannerSegmask.srv index aa31b5e3..23c79ac7 100644 --- a/srv/GQCNNGraspPlannerSegmask.srv +++ b/srv/GQCNNGraspPlannerSegmask.srv @@ -1,3 +1,25 @@ +# Copyright ©2017. The Regents of the University of California (Regents). +# All Rights Reserved. Permission to use, copy, modify, and distribute this +# software and its documentation for educational, research, and not-for-profit +# purposes, without fee and without a signed licensing agreement, is hereby +# granted, provided that the above copyright notice, this paragraph and the +# following two paragraphs appear in all copies, modifications, and +# distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 +# Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, +# otl@berkeley.edu, +# http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. + +# IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, +# INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF +# THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN +# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +# THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +# PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED +# HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE +# MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. + # request params sensor_msgs/Image color_image sensor_msgs/Image depth_image @@ -5,4 +27,4 @@ sensor_msgs/CameraInfo camera_info sensor_msgs/Image segmask --- # response params -GQCNNGrasp grasp \ No newline at end of file +GQCNNGrasp grasp From c2a690da357b6e23cca14d1ec546251d05d9b477 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Mon, 27 May 2019 21:34:54 -0700 Subject: [PATCH 15/29] Fixing bug in setup.py. --- setup.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/setup.py b/setup.py index 33ebe366..059765c7 100644 --- a/setup.py +++ b/setup.py @@ -51,14 +51,13 @@ def get_tf_dep(): # Check whether or not the Nvidia driver and GPUs are available and add the # corresponding Tensorflow dependency. - tf_dep = "tensorflow<={}".format(TF_MIN_VERSION, TF_MAX_VERSION) + tf_dep = "tensorflow<={}".format(TF_MAX_VERSION) try: gpus = subprocess.check_output( ["nvidia-smi", "--query-gpu=gpu_name", "--format=csv"]).decode().strip().split("\n")[1:] if len(gpus) > 0: - tf_dep = "tensorflow-gpu<={}".format(TF_MIN_VERSION, - TF_MAX_VERSION) + tf_dep = "tensorflow-gpu<={}".format(TF_MAX_VERSION) else: no_device_msg = ("Found Nvidia device driver but no" " devices...installing Tensorflow for CPU.") From 4b40f50b820dedb7fe6ec3196e7b071319238e15 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Mon, 27 May 2019 21:46:30 -0700 Subject: [PATCH 16/29] Downgrading allowed matplotlib version for compatibility with other AUTOLab libraries. --- requirements/cpu_requirements.txt | 2 +- requirements/gpu_requirements.txt | 2 +- setup.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/requirements/cpu_requirements.txt b/requirements/cpu_requirements.txt index e5ead1ca..16837d6a 100644 --- a/requirements/cpu_requirements.txt +++ b/requirements/cpu_requirements.txt @@ -4,7 +4,7 @@ visualization numpy opencv-python scipy -matplotlib +matplotlib<=2.2.0 tensorflow<=1.13.1 scikit-learn scikit-image<=0.14.2 diff --git a/requirements/gpu_requirements.txt b/requirements/gpu_requirements.txt index 3430d188..4bdbe37a 100644 --- a/requirements/gpu_requirements.txt +++ b/requirements/gpu_requirements.txt @@ -4,7 +4,7 @@ visualization numpy opencv-python scipy -matplotlib +matplotlib<=2.2.0 tensorflow-gpu<=1.13.1 scikit-learn scikit-image<=0.14.2 diff --git a/setup.py b/setup.py index 059765c7..bba49bca 100644 --- a/setup.py +++ b/setup.py @@ -142,7 +142,7 @@ def run(self): requirements = [ "autolab-core", "autolab-perception", "visualization", "numpy", "scipy", - "matplotlib", "opencv-python", "scikit-image<=0.14.2", "scikit-learn", "psutil", + "matplotlib<=2.2.0", "opencv-python", "scikit-image<=0.14.2", "scikit-learn", "psutil", "gputil" ] From 75542fb23ef438e9e4dc889a443b519d9a20f7d9 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 07:36:11 -0700 Subject: [PATCH 17/29] Fixing minor bugs and cleaning up some files. --- docker/cpu/Dockerfile | 12 ++++++------ docker/gpu/Dockerfile | 12 ++++++------ gqcnn/analysis/analyzer.py | 2 +- gqcnn/grasping/grasp.py | 2 +- gqcnn/model/tf/network_tf.py | 6 +++--- gqcnn/training/tf/trainer_tf.py | 30 +++++++++++++++--------------- gqcnn/utils/enums.py | 1 + 7 files changed, 33 insertions(+), 32 deletions(-) diff --git a/docker/cpu/Dockerfile b/docker/cpu/Dockerfile index 5dfb4278..8ecc68b0 100644 --- a/docker/cpu/Dockerfile +++ b/docker/cpu/Dockerfile @@ -3,10 +3,10 @@ FROM ubuntu:xenial MAINTAINER Vishal Satish # Args -# work_dir must be an absolute path +# `work_dir` must be an absolute path. ARG work_dir=/root/Workspace -# Install apt-get deps +# Install `apt-get` deps. RUN apt-get update && apt-get install -y \ build-essential \ python-tk \ @@ -20,18 +20,18 @@ RUN apt-get update && apt-get install -y \ wget \ unzip -# Install pip (`apt-get install python-pip` causes trouble w/ networkx) +# Install pip (`apt-get install python-pip` causes trouble w/ networkx). RUN curl -O https://bootstrap.pypa.io/get-pip.py && \ python get-pip.py && \ rm get-pip.py -# Make working directory +# Make working directory. WORKDIR ${work_dir} -# Copy the library +# Copy the library. ADD docker/gqcnn.tar . -# This is because `python setup.py develop` skips install_requires (I think) and also because we want to explicitly use the GPU requirements +# This is because `python setup.py develop` skips install_requires (I think) and also because we want to explicitly use the GPU requirements. RUN pip install -r gqcnn/requirements/cpu_requirements.txt # Install the library in editable mode because it's more versatile (in case we want to develop or if users want to modify things) diff --git a/docker/gpu/Dockerfile b/docker/gpu/Dockerfile index 2b8a6396..79409aef 100644 --- a/docker/gpu/Dockerfile +++ b/docker/gpu/Dockerfile @@ -3,10 +3,10 @@ FROM nvidia/cuda:10.0-cudnn7-devel-ubuntu16.04 MAINTAINER Vishal Satish # Args -# work_dir must be an absolute path +# `work_dir` must be an absolute path. ARG work_dir=/root/Workspace -# Install apt-get deps +# Install `apt-get` deps. RUN apt-get update && apt-get install -y \ build-essential \ python3.7 \ @@ -21,18 +21,18 @@ RUN apt-get update && apt-get install -y \ wget \ unzip -# Install pip (`apt-get install python-pip` causes trouble w/ networkx) +# Install pip (`apt-get install python-pip` causes trouble w/ networkx). RUN curl -O https://bootstrap.pypa.io/get-pip.py && \ python get-pip.py && \ rm get-pip.py -# Make working directory +# Make working directory. WORKDIR ${work_dir} -# Copy the library +# Copy the library. ADD docker/gqcnn.tar . -# This is because `python setup.py develop` skips install_requires (I think) and also because we want to explicitly use the GPU requirements +# This is because `python setup.py develop` skips install_requires (I think) and also because we want to explicitly use the GPU requirements. RUN pip install -r gqcnn/requirements/gpu_requirements.txt # Install the library in editable mode because it's more versatile (in case we want to develop or if users want to modify things) diff --git a/gqcnn/analysis/analyzer.py b/gqcnn/analysis/analyzer.py index b2d5e0ef..e1b9b01d 100644 --- a/gqcnn/analysis/analyzer.py +++ b/gqcnn/analysis/analyzer.py @@ -712,7 +712,7 @@ def _plot(self, model_dir, model_output_dir, train_result, val_result): pct_pos_val_filename = os.path.join(model_dir, GQCNNFilenames.PCT_POS_VAL) train_losses_filename = os.path.join(model_dir, - GQCNNFilenames.TRAIN_LOSS) + GQCNNFilenames.TRAIN_LOSSES) raw_train_errors = np.load(train_errors_filename) val_errors = np.load(val_errors_filename) diff --git a/gqcnn/grasping/grasp.py b/gqcnn/grasping/grasp.py index d4b430ba..0a6f50f6 100644 --- a/gqcnn/grasping/grasp.py +++ b/gqcnn/grasping/grasp.py @@ -373,7 +373,7 @@ def from_feature_vec(v, camera_intr=None, depth=None, axis=None): center_px = v[:2] grasp_axis = np.array([0, 0, -1]) - if v.shape > 2 and axis is None: + if len(v.shape) > 2 and axis is None: grasp_axis = v[2:5] grasp_axis = grasp_axis / np.linalg.norm(grasp_axis) elif axis is not None: diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index 8074fec4..a9c7c5f5 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -311,20 +311,20 @@ def set_base_network(self, model_dir): if use_legacy: layer_iter = iter(base_arch) while not found_base_layer: - layer_name = layer_iter.next() + layer_name = next(layer_iter) self._base_layer_names.append(layer_name) if layer_name == output_layer: found_base_layer = True else: stream_iter = iter(base_arch) while not found_base_layer: - stream_name = stream_iter.next() + stream_name = next(stream_iter) stream_arch = base_arch[stream_name] layer_iter = iter(stream_arch) stop = False while not found_base_layer and not stop: try: - layer_name = layer_iter.next() + layer_name = next(layer_iter) self._base_layer_names.append(layer_name) if layer_name == output_layer: found_base_layer = True diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index 1e6ed870..77cc2e1d 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -424,7 +424,7 @@ def handler(signum, frame): options=GeneralConstants.timeout_option) step_stop = time.time() self.logger.info("Step took {} sec.".format( - round(step_stop - step_start, 3))) + str(round(step_stop - step_start, 3)))) if self.training_mode == TrainingMode.REGRESSION: self.logger.info("Max " + str(np.max(predictions))) @@ -463,15 +463,15 @@ def handler(signum, frame): start_time = time.time() self.logger.info("Step {} (epoch {}), {} s".format( step, - round(step * self.train_batch_size / self.num_train, - 3), - round(1000 * elapsed_time / self.eval_frequency, 2))) + str(round(step * self.train_batch_size / self.num_train, + 3)), + str(round(1000 * elapsed_time / self.eval_frequency, 2)))) self.logger.info( "Minibatch loss: {}, learning rate: {}".format( - round(l, 3), round(lr, 6))) + str(round(l, 3)), str(round(lr, 6)))) if self.progress_dict is not None: - self.progress_dict["epoch"] = round( - step * self.train_batch_size / self.num_train, 2) + self.progress_dict["epoch"] = str(round( + step * self.train_batch_size / self.num_train, 2)) train_error = l if self.training_mode == TrainingMode.CLASSIFICATION: @@ -483,7 +483,7 @@ def handler(signum, frame): train_error = classification_result.error_rate self.logger.info("Minibatch error: {}".format( - round(train_error, 3))) + str(round(train_error, 3)))) self.summary_writer.add_summary( self.sess.run( @@ -510,7 +510,7 @@ def handler(signum, frame): train_result = self._error_rate_in_batches( validation_set=False) self.logger.info("Training error: {}".format( - round(train_result.error_rate, 3))) + str(round(train_result.error_rate, 3)))) # Update the `TrainStatsLogger` and save. self.train_stats_logger.update( @@ -534,9 +534,9 @@ def handler(signum, frame): error_rate }), step) self.logger.info("Validation error: {}".format( - round(val_result.error_rate, 3))) + str(round(val_result.error_rate, 3)))) self.logger.info("Validation loss: {}".format( - round(val_result.cross_entropy_loss, 3))) + str(round(val_result.cross_entropy_loss, 3)))) sys.stdout.flush() # Update the `TrainStatsLogger`. @@ -580,9 +580,9 @@ def handler(signum, frame): # Get final errors and flush the stdout pipeline. final_val_result = self._error_rate_in_batches() self.logger.info("Final validation error: {}".format( - round(final_val_result.error_rate, 3))) + str(round(final_val_result.error_rate, 3)))) self.logger.info("Final validation loss: {}".format( - round(final_val_result.cross_entropy_loss, 3))) + str(round(final_val_result.cross_entropy_loss, 3)))) if self.cfg["eval_total_train_error"]: final_train_result = self._error_rate_in_batches( validation_set=False) @@ -1424,7 +1424,7 @@ def _load_and_enqueue(self, seed): file_num) read_stop = time.time() self.logger.debug("Reading data took {} sec".format( - round(read_stop - read_start, 3))) + str(round(read_stop - read_start, 3)))) self.logger.debug("File num: {}".format(file_num)) # Get batch indices uniformly at random. @@ -1552,7 +1552,7 @@ def _load_and_enqueue(self, seed): time.sleep(GeneralConstants.QUEUE_SLEEP) queue_stop = time.time() self.logger.debug("Queue batch took {} sec".format( - round(queue_stop - queue_start, 3))) + str(round(queue_stop - queue_start, 3)))) def _distort(self, image_arr, pose_arr): """Adds noise to a batch of images.""" diff --git a/gqcnn/utils/enums.py b/gqcnn/utils/enums.py index 0cce78a7..3f88838d 100644 --- a/gqcnn/utils/enums.py +++ b/gqcnn/utils/enums.py @@ -93,6 +93,7 @@ class GQCNNTrainingStatus(object): # Enum for filenames. class GQCNNFilenames(object): PCT_POS_VAL = "pct_pos_val.npy" + PCT_POS_TRAIN = "pct_pos_train.npy" LEARNING_RATES = "learning_rates.npy" TRAIN_ITERS = "train_eval_iters.npy" From 6292ed4cc15f72baf659b0865e863f8c9a0f0da1 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 11:35:06 -0700 Subject: [PATCH 18/29] Explicitly declaring meshrender==0.0.9 dep. --- requirements/cpu_requirements.txt | 1 + requirements/gpu_requirements.txt | 1 + setup.py | 2 +- 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/requirements/cpu_requirements.txt b/requirements/cpu_requirements.txt index 16837d6a..cfbf5fbb 100644 --- a/requirements/cpu_requirements.txt +++ b/requirements/cpu_requirements.txt @@ -1,5 +1,6 @@ autolab-core autolab-perception +meshrender==0.0.9 visualization numpy opencv-python diff --git a/requirements/gpu_requirements.txt b/requirements/gpu_requirements.txt index 4bdbe37a..72a6b8ff 100644 --- a/requirements/gpu_requirements.txt +++ b/requirements/gpu_requirements.txt @@ -1,5 +1,6 @@ autolab-core autolab-perception +meshrender==0.0.9 visualization numpy opencv-python diff --git a/setup.py b/setup.py index bba49bca..3a379799 100644 --- a/setup.py +++ b/setup.py @@ -141,7 +141,7 @@ def run(self): requirements = [ - "autolab-core", "autolab-perception", "visualization", "numpy", "scipy", + "autolab-core", "autolab-perception", "meshrender==0.0.9", "visualization", "numpy", "scipy", "matplotlib<=2.2.0", "opencv-python", "scikit-image<=0.14.2", "scikit-learn", "psutil", "gputil" ] From 8f5639f71b127591a61beab0810b35239799f9ec Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 11:50:08 -0700 Subject: [PATCH 19/29] Adding GL install to Travis config. --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 23313ef6..54fb449e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -53,6 +53,7 @@ matrix: before_install: - sudo apt-get update - sudo apt-get install python-opengl # For GLU. + - sudo apt-get install freeglut-dev # For GL. install: - pip install . From 03ea209c26c533611d16ad0674e26624be9800ce Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 11:52:46 -0700 Subject: [PATCH 20/29] Updating freeglut-dev to freeglut3-dev. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 54fb449e..0413237c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -53,7 +53,7 @@ matrix: before_install: - sudo apt-get update - sudo apt-get install python-opengl # For GLU. - - sudo apt-get install freeglut-dev # For GL. + - sudo apt-get install freeglut3-dev # For GL. install: - pip install . From 2fd807d7c188696a7d8224735a0a062370275408 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 12:09:53 -0700 Subject: [PATCH 21/29] Testing Travis OpenGL. --- .travis.yml | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0413237c..ca8e2344 100644 --- a/.travis.yml +++ b/.travis.yml @@ -55,11 +55,19 @@ before_install: - sudo apt-get install python-opengl # For GLU. - sudo apt-get install freeglut3-dev # For GL. -install: - - pip install . +#install: +# - pip install . + +#script: +# - python -c "import gqcnn" + +install: + - pip install PyOpenGL + - pip install pyglet script: - - python -c "import gqcnn" + - python -c "import OpenGL" + - python -c "import pyglet" notifications: email: false From bb82b67710202e33dce63013d28cf1892daa8967 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 13:25:06 -0700 Subject: [PATCH 22/29] Made meshrender dep conditional on Python 2. --- .travis.yml | 15 +++------------ setup.py | 7 ++++++- 2 files changed, 9 insertions(+), 13 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca8e2344..23313ef6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -53,21 +53,12 @@ matrix: before_install: - sudo apt-get update - sudo apt-get install python-opengl # For GLU. - - sudo apt-get install freeglut3-dev # For GL. -#install: -# - pip install . - -#script: -# - python -c "import gqcnn" - -install: - - pip install PyOpenGL - - pip install pyglet +install: + - pip install . script: - - python -c "import OpenGL" - - python -c "import pyglet" + - python -c "import gqcnn" notifications: email: false diff --git a/setup.py b/setup.py index 3a379799..38a9b1ec 100644 --- a/setup.py +++ b/setup.py @@ -141,11 +141,16 @@ def run(self): requirements = [ - "autolab-core", "autolab-perception", "meshrender==0.0.9", "visualization", "numpy", "scipy", + "autolab-core", "autolab-perception", "visualization", "numpy", "scipy", "matplotlib<=2.2.0", "opencv-python", "scikit-image<=0.14.2", "scikit-learn", "psutil", "gputil" ] +if sys.version[0] == "2": + # The Python 2 meshrender installation is broken, and we don't know + # when it will be fixed. + requirements.append("meshrender==0.0.9") + exec( open( os.path.join(os.path.dirname(os.path.realpath(__file__)), From f3f4b13064e5c1e2ee86319eac9458a27872d834 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 13:30:35 -0700 Subject: [PATCH 23/29] Patching PR #73. --- gqcnn/grasping/grasp.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gqcnn/grasping/grasp.py b/gqcnn/grasping/grasp.py index 0a6f50f6..02199026 100644 --- a/gqcnn/grasping/grasp.py +++ b/gqcnn/grasping/grasp.py @@ -373,7 +373,7 @@ def from_feature_vec(v, camera_intr=None, depth=None, axis=None): center_px = v[:2] grasp_axis = np.array([0, 0, -1]) - if len(v.shape) > 2 and axis is None: + if v.shape[0] > 2 and axis is None: grasp_axis = v[2:5] grasp_axis = grasp_axis / np.linalg.norm(grasp_axis) elif axis is not None: @@ -591,7 +591,7 @@ def from_feature_vec(v, center_px = v[:2] grasp_angle = 0 - if v.shape > 2 and angle is None: + if v.shape[0] > 2 and angle is None: # grasp_angle = v[2] grasp_vec = v[2:] grasp_vec = grasp_vec / np.linalg.norm(grasp_vec) From 28c60ed50e653cdd714a7aa296aa9cb8fa2f5596 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 13:41:50 -0700 Subject: [PATCH 24/29] Updating meshrender dep stuff. --- requirements/{ => p2}/cpu_requirements.txt | 0 requirements/{ => p2}/gpu_requirements.txt | 0 requirements/p3/cpu_requirements.txt | 12 ++++++++++++ requirements/p3/gpu_requirements.txt | 12 ++++++++++++ setup.py | 4 ++-- 5 files changed, 26 insertions(+), 2 deletions(-) rename requirements/{ => p2}/cpu_requirements.txt (100%) rename requirements/{ => p2}/gpu_requirements.txt (100%) create mode 100644 requirements/p3/cpu_requirements.txt create mode 100644 requirements/p3/gpu_requirements.txt diff --git a/requirements/cpu_requirements.txt b/requirements/p2/cpu_requirements.txt similarity index 100% rename from requirements/cpu_requirements.txt rename to requirements/p2/cpu_requirements.txt diff --git a/requirements/gpu_requirements.txt b/requirements/p2/gpu_requirements.txt similarity index 100% rename from requirements/gpu_requirements.txt rename to requirements/p2/gpu_requirements.txt diff --git a/requirements/p3/cpu_requirements.txt b/requirements/p3/cpu_requirements.txt new file mode 100644 index 00000000..16837d6a --- /dev/null +++ b/requirements/p3/cpu_requirements.txt @@ -0,0 +1,12 @@ +autolab-core +autolab-perception +visualization +numpy +opencv-python +scipy +matplotlib<=2.2.0 +tensorflow<=1.13.1 +scikit-learn +scikit-image<=0.14.2 +gputil +psutil diff --git a/requirements/p3/gpu_requirements.txt b/requirements/p3/gpu_requirements.txt new file mode 100644 index 00000000..4bdbe37a --- /dev/null +++ b/requirements/p3/gpu_requirements.txt @@ -0,0 +1,12 @@ +autolab-core +autolab-perception +visualization +numpy +opencv-python +scipy +matplotlib<=2.2.0 +tensorflow-gpu<=1.13.1 +scikit-learn +scikit-image<=0.14.2 +gputil +psutil diff --git a/setup.py b/setup.py index 38a9b1ec..c2fd3ad2 100644 --- a/setup.py +++ b/setup.py @@ -147,8 +147,8 @@ def run(self): ] if sys.version[0] == "2": - # The Python 2 meshrender installation is broken, and we don't know - # when it will be fixed. + # The Python 2 installation for the latest version of meshrender + # is broken, and we don't know when it will be fixed. requirements.append("meshrender==0.0.9") exec( From 6fe2382471c7cd18148bc7c70491411c117d22db Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 13:49:40 -0700 Subject: [PATCH 25/29] Bumping Python 2.7 Travis build to Xenial. --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 23313ef6..d4f5ef1b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -23,9 +23,9 @@ language: python matrix: include: - - name: "Python 2.7 on Trusty Linux" + - name: "Python 2.7 on Xenial Linux" python: 2.7 - dist: trusty + dist: xenial - name: "Python 3.5 on Xenial Linux" python: 3.5 From 5575227469dea73b060e430539dff1e88959ea94 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 14:01:50 -0700 Subject: [PATCH 26/29] Fixing formatting. --- gqcnn/grasping/policy/fc_policy.py | 4 +++- gqcnn/grasping/policy/policy.py | 18 +++++++++++++----- gqcnn/training/tf/trainer_tf.py | 15 ++++++++++----- setup.py | 4 ++-- 4 files changed, 28 insertions(+), 13 deletions(-) diff --git a/gqcnn/grasping/policy/fc_policy.py b/gqcnn/grasping/policy/fc_policy.py index bc64d262..92d7e791 100644 --- a/gqcnn/grasping/policy/fc_policy.py +++ b/gqcnn/grasping/policy/fc_policy.py @@ -303,7 +303,9 @@ def _action(self, state, num_actions=1): # Filter grasps. if self._filter_grasps: - actions = sorted(actions, reverse=True, key=lambda action: action.q_value) + actions = sorted(actions, + reverse=True, + key=lambda action: action.q_value) actions = [self._filter(actions)] # Visualize. diff --git a/gqcnn/grasping/policy/policy.py b/gqcnn/grasping/policy/policy.py index 5336d564..01187f0f 100644 --- a/gqcnn/grasping/policy/policy.py +++ b/gqcnn/grasping/policy/policy.py @@ -565,7 +565,9 @@ def select(self, grasps, q_value): # Sort grasps. num_grasps = len(grasps) grasps_and_predictions = zip(np.arange(num_grasps), q_value) - grasps_and_predictions = sorted(grasps_and_predictions, key=lambda x: x[1], reverse=True) + grasps_and_predictions = sorted(grasps_and_predictions, + key=lambda x: x[1], + reverse=True) # Return top grasps. if self._filters is None: @@ -795,7 +797,9 @@ def select(self, grasps, q_values): if num_grasps == 0: raise NoValidGraspsException("Zero grasps") grasps_and_predictions = zip(np.arange(num_grasps), q_values) - grasps_and_predictions = sorted(grasps_and_predictions, key=lambda x: x[1], reverse=True) + grasps_and_predictions = sorted(grasps_and_predictions, + key=lambda x: x[1], + reverse=True) # Return top grasps. if self._filters is None: @@ -1039,7 +1043,9 @@ def action_set(self, state): # Sort grasps. resample_start = time() q_values_and_indices = zip(q_values, np.arange(num_grasps)) - q_values_and_indices = sorted(q_values_and_indices, key=lambda x: x[0], reverse=True) + q_values_and_indices = sorted(q_values_and_indices, + key=lambda x: x[0], + reverse=True) if self.config["vis"]["grasp_candidates"]: # Display each grasp on the original image, colored by @@ -1059,7 +1065,8 @@ def action_set(self, state): save_fname="cem_iter_{}.png".format(j), save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) - display_grasps_and_q_values = sorted(display_grasps_and_q_values, key=lambda x: x[1]) + display_grasps_and_q_values = sorted( + display_grasps_and_q_values, key=lambda x: x[1]) vis.figure(size=(GeneralConstants.FIGSIZE, GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, @@ -1241,7 +1248,8 @@ def action_set(self, state): save_fname="final_sampled_grasps.png".format(j), save_path=state_output_dir) display_grasps_and_q_values = zip(grasps, q_values) - display_grasps_and_q_values = sorted(display_grasps_and_q_values, key=lambda x: x[1]) + display_grasps_and_q_values = sorted(display_grasps_and_q_values, + key=lambda x: x[1]) vis.figure(size=(GeneralConstants.FIGSIZE, GeneralConstants.FIGSIZE)) vis.imshow(rgbd_im.depth, diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index 77cc2e1d..8f2d4755 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -463,15 +463,20 @@ def handler(signum, frame): start_time = time.time() self.logger.info("Step {} (epoch {}), {} s".format( step, - str(round(step * self.train_batch_size / self.num_train, - 3)), - str(round(1000 * elapsed_time / self.eval_frequency, 2)))) + str( + round( + step * self.train_batch_size / self.num_train, + 3)), + str(round(1000 * elapsed_time / self.eval_frequency, + 2)))) self.logger.info( "Minibatch loss: {}, learning rate: {}".format( str(round(l, 3)), str(round(lr, 6)))) if self.progress_dict is not None: - self.progress_dict["epoch"] = str(round( - step * self.train_batch_size / self.num_train, 2)) + self.progress_dict["epoch"] = str( + round( + step * self.train_batch_size / self.num_train, + 2)) train_error = l if self.training_mode == TrainingMode.CLASSIFICATION: diff --git a/setup.py b/setup.py index c2fd3ad2..7f758e9e 100644 --- a/setup.py +++ b/setup.py @@ -142,8 +142,8 @@ def run(self): requirements = [ "autolab-core", "autolab-perception", "visualization", "numpy", "scipy", - "matplotlib<=2.2.0", "opencv-python", "scikit-image<=0.14.2", "scikit-learn", "psutil", - "gputil" + "matplotlib<=2.2.0", "opencv-python", "scikit-image<=0.14.2", + "scikit-learn", "psutil", "gputil" ] if sys.version[0] == "2": From 20f5a45f1b2d0ec4b4b02120b3173751c3ca3bf7 Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Wed, 29 May 2019 18:17:18 -0700 Subject: [PATCH 27/29] Added flake8 config and replaced escaped double quotes with single quotes. --- .flake8 | 5 +++++ .travis.yml | 2 +- gqcnn/model/tf/network_tf.py | 4 ++-- gqcnn/search/utils.py | 2 +- gqcnn/training/tf/trainer_tf.py | 6 +++--- gqcnn/utils/utils.py | 4 ++-- tools/hyperparam_search.py | 2 +- 7 files changed, 15 insertions(+), 10 deletions(-) create mode 100644 .flake8 diff --git a/.flake8 b/.flake8 new file mode 100644 index 00000000..087bea7c --- /dev/null +++ b/.flake8 @@ -0,0 +1,5 @@ +[flake8] +ignore = C408, E121, E123, E126, E226, E24, E704, W503, W504, W605 +exclude = docs/source/conf.py +inline-quotes = " +no-avoid-escape = 1 diff --git a/.travis.yml b/.travis.yml index d4f5ef1b..ea7ae113 100644 --- a/.travis.yml +++ b/.travis.yml @@ -48,7 +48,7 @@ matrix: - pip install yapf==0.27.0 # Must use specific version. - pip install flake8 flake8-comprehensions flake8-quotes==2.0.0 - ./ci/travis/format.sh --all # Should exit with 0 for no diffs. - - flake8 --inline-quotes '"' --no-avoid-escape --exclude=docs/source/conf.py --ignore=C408,E121,E123,E126,E226,E24,E704,W503,W504,W605 + - flake8 before_install: - sudo apt-get update diff --git a/gqcnn/model/tf/network_tf.py b/gqcnn/model/tf/network_tf.py index a9c7c5f5..ca2669ef 100644 --- a/gqcnn/model/tf/network_tf.py +++ b/gqcnn/model/tf/network_tf.py @@ -1394,7 +1394,7 @@ def _build_network(self, input_im_node, input_pose_node, self._logger.info("Building Network...") if self._input_depth_mode == InputDepthMode.POSE_STREAM: missing_stream_msg = ("When using input depth mode" - " \"pose_stream\", both pose stream and" + " 'pose_stream', both pose stream and" " merge stream must be present!") assert "pose_stream" in self._architecture and \ "merge_stream" in self._architecture, missing_stream_msg @@ -1414,7 +1414,7 @@ def _build_network(self, input_im_node, input_pose_node, self._architecture["merge_stream"])[0] elif self._input_depth_mode == InputDepthMode.SUB or \ self._input_depth_mode == InputDepthMode.IM_ONLY: - extraneous_stream_msg = ("When using input depth mode \"{}\", only" + extraneous_stream_msg = ("When using input depth mode '{}', only" " im stream is allowed!") assert not ("pose_stream" in self._architecture or "merge_stream" in self._architecture diff --git a/gqcnn/search/utils.py b/gqcnn/search/utils.py index 40e6dfbf..673d43db 100644 --- a/gqcnn/search/utils.py +++ b/gqcnn/search/utils.py @@ -101,7 +101,7 @@ def parse_master_train_config(train_config): num_params = [] for field in fields: num_params.append(len(get_nested_key(train_config, field))) - invalid_anchor_tag_msg = ("All fields in anchor tag \"{}\" do not have" + invalid_anchor_tag_msg = ("All fields in anchor tag '{}' do not have" " the same # of parameters to search over!") assert max(num_params) == min( num_params), invalid_anchor_tag_msg.format(anchor_tag) diff --git a/gqcnn/training/tf/trainer_tf.py b/gqcnn/training/tf/trainer_tf.py index 8f2d4755..9b8c756b 100644 --- a/gqcnn/training/tf/trainer_tf.py +++ b/gqcnn/training/tf/trainer_tf.py @@ -195,7 +195,7 @@ def _create_optimizer(self, loss, batch, var_list, learning_rate): elif self.cfg["optimizer"] == "rmsprop": optimizer = tf.train.RMSPropOptimizer(learning_rate) else: - raise ValueError("Optimizer \"{}\" not supported".format( + raise ValueError("Optimizer '{}' not supported".format( self.cfg["optimizer"])) # Compute gradients. @@ -1095,7 +1095,7 @@ def _read_training_params(self): self.training_mode = self.cfg["training_mode"] if self.training_mode != TrainingMode.CLASSIFICATION: raise ValueError( - "Training mode \"{}\" not currently supported!".format( + "Training mode '{}' not currently supported!".format( self.training_mode)) # Tensorboad. @@ -1252,7 +1252,7 @@ def _setup_tensorflow(self): train_label_dtype = tf.float32 self.numpy_dtype = np.float32 else: - raise ValueError("Training mode \"{}\" not supported".format( + raise ValueError("Training mode '{}' not supported".format( self.training_mode)) # Set up placeholders. diff --git a/gqcnn/utils/utils.py b/gqcnn/utils/utils.py index 349a9016..225903a5 100644 --- a/gqcnn/utils/utils.py +++ b/gqcnn/utils/utils.py @@ -101,7 +101,7 @@ def pose_dim(gripper_mode): return 2 else: raise ValueError( - "Gripper mode \"{}\" not supported.".format(gripper_mode)) + "Gripper mode '{}' not supported.".format(gripper_mode)) def read_pose_data(pose_arr, gripper_mode): @@ -147,7 +147,7 @@ def read_pose_data(pose_arr, gripper_mode): return pose_arr[:, 2:4] else: raise ValueError( - "Gripper mode \"{}\" not supported.".format(gripper_mode)) + "Gripper mode '{}' not supported.".format(gripper_mode)) def reduce_shape(shape): diff --git a/tools/hyperparam_search.py b/tools/hyperparam_search.py index 907dcefa..d035a3a1 100644 --- a/tools/hyperparam_search.py +++ b/tools/hyperparam_search.py @@ -103,7 +103,7 @@ if len(split_names) < len(datasets): if len(split_names) == 1: logger.warning( - "Using split \"{}\" for all datasets/configs...".format( + "Using split '{}' for all datasets/configs...".format( split_names[0])) split_names *= len(datasets) else: From af8c152e950268ea5c5bb6f5c3f9afbbe6080f1a Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Thu, 30 May 2019 11:36:21 -0700 Subject: [PATCH 28/29] WIP Updating docker for Python 2/3. --- .travis.yml | 2 +- docker/cpu/Dockerfile | 22 ++++++++++++++-------- docker/gpu/Dockerfile | 13 +++++++++---- gqcnn/version.py | 28 ---------------------------- 4 files changed, 24 insertions(+), 41 deletions(-) diff --git a/.travis.yml b/.travis.yml index ea7ae113..971b3dd7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -52,7 +52,7 @@ matrix: before_install: - sudo apt-get update - - sudo apt-get install python-opengl # For GLU. + - sudo apt-get install -y python-opengl # For GLU. install: - pip install . diff --git a/docker/cpu/Dockerfile b/docker/cpu/Dockerfile index 8ecc68b0..7adb3104 100644 --- a/docker/cpu/Dockerfile +++ b/docker/cpu/Dockerfile @@ -2,15 +2,16 @@ FROM ubuntu:xenial MAINTAINER Vishal Satish -# Args -# `work_dir` must be an absolute path. -ARG work_dir=/root/Workspace +# Args. +# Must be an absolute path. +ARG work_dir=/root/Workspace # Install `apt-get` deps. RUN apt-get update && apt-get install -y \ build-essential \ - python-tk \ + python3 \ python-dev \ + python3-dev \ python-opengl \ curl \ libsm6 \ @@ -24,6 +25,9 @@ RUN apt-get update && apt-get install -y \ RUN curl -O https://bootstrap.pypa.io/get-pip.py && \ python get-pip.py && \ rm get-pip.py +RUN curl -O https://bootstrap.pypa.io/get-pip.py && \ + python3 get-pip.py && \ + rm get-pip.py # Make working directory. WORKDIR ${work_dir} @@ -31,15 +35,17 @@ WORKDIR ${work_dir} # Copy the library. ADD docker/gqcnn.tar . -# This is because `python setup.py develop` skips install_requires (I think) and also because we want to explicitly use the GPU requirements. -RUN pip install -r gqcnn/requirements/cpu_requirements.txt +# This is because `python setup.py develop` skips `install_requires` (I think). +RUN python -m pip install -r gqcnn/requirements/p2/cpu_requirements.txt +RUN python3 -m pip install -r gqcnn/requirements/p3/cpu_requirements.txt # Install the library in editable mode because it's more versatile (in case we want to develop or if users want to modify things) # Keep the egg outside of the library in site-packages in case we want to mount the library (overwriting it) for development with docker ENV PYTHONPATH ${work_dir}/gqcnn WORKDIR /usr/local/lib/python2.7/site-packages/ RUN python ${work_dir}/gqcnn/setup.py develop --docker +WORKDIR /usr/local/lib/python3.5/site-packages/ +RUN python3 ${work_dir}/gqcnn/setup.py develop --docker -# Move to the base library dir +# Move to the top-level gqcnn package dir. WORKDIR ${work_dir}/gqcnn - diff --git a/docker/gpu/Dockerfile b/docker/gpu/Dockerfile index 79409aef..d621927d 100644 --- a/docker/gpu/Dockerfile +++ b/docker/gpu/Dockerfile @@ -9,9 +9,9 @@ ARG work_dir=/root/Workspace # Install `apt-get` deps. RUN apt-get update && apt-get install -y \ build-essential \ - python3.7 \ - python-tk \ + python3 \ python-dev \ + python3-dev \ python-opengl \ curl \ libsm6 \ @@ -25,6 +25,9 @@ RUN apt-get update && apt-get install -y \ RUN curl -O https://bootstrap.pypa.io/get-pip.py && \ python get-pip.py && \ rm get-pip.py +RUN curl -O https://bootstrap.pypa.io/get-pip.py && \ + python3 get-pip.py && \ + rm get-pip.py # Make working directory. WORKDIR ${work_dir} @@ -33,14 +36,16 @@ WORKDIR ${work_dir} ADD docker/gqcnn.tar . # This is because `python setup.py develop` skips install_requires (I think) and also because we want to explicitly use the GPU requirements. -RUN pip install -r gqcnn/requirements/gpu_requirements.txt +RUN python -m pip install -r gqcnn/requirements/p2/gpu_requirements.txt +RUN python3 -m pip install -r gqcnn/requirements/p3/gpu_requirements.txt # Install the library in editable mode because it's more versatile (in case we want to develop or if users want to modify things) # Keep the egg outside of the library in site-packages in case we want to mount the library (overwriting it) for development with docker ENV PYTHONPATH ${work_dir}/gqcnn WORKDIR /usr/local/lib/python2.7/site-packages/ RUN python ${work_dir}/gqcnn/setup.py develop --docker +WORKDIR /usr/local/lib/python3.5/site-packages/ +RUN python3 ${work_dir}/gqcnn/setup.py develop --docker # Move to the base library dir WORKDIR ${work_dir}/gqcnn - diff --git a/gqcnn/version.py b/gqcnn/version.py index 5e44327d..6849410a 100644 --- a/gqcnn/version.py +++ b/gqcnn/version.py @@ -1,29 +1 @@ -# -*- coding: utf-8 -*- -""" -Copyright ©2017. The Regents of the University of California (Regents). -All Rights Reserved. Permission to use, copy, modify, and distribute this -software and its documentation for educational, research, and not-for-profit -purposes, without fee and without a signed licensing agreement, is hereby -granted, provided that the above copyright notice, this paragraph and the -following two paragraphs appear in all copies, modifications, and -distributions. Contact The Office of Technology Licensing, UC Berkeley, 2150 -Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-7201, -otl@berkeley.edu, -http://ipira.berkeley.edu/industry-info for commercial licensing opportunities. - -IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, -INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF -THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN -ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED -HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE -MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. -""" -from __future__ import absolute_import -from __future__ import division -from __future__ import print_function - __version__ = "1.1.0" From 699b40779c68feac350ccc24a76d951701f4bdfc Mon Sep 17 00:00:00 2001 From: Vishal Satish Date: Thu, 30 May 2019 11:59:46 -0700 Subject: [PATCH 29/29] Added Tkinter deps. --- docker/cpu/Dockerfile | 2 ++ docker/gpu/Dockerfile | 2 ++ 2 files changed, 4 insertions(+) diff --git a/docker/cpu/Dockerfile b/docker/cpu/Dockerfile index 7adb3104..c44013c1 100644 --- a/docker/cpu/Dockerfile +++ b/docker/cpu/Dockerfile @@ -12,6 +12,8 @@ RUN apt-get update && apt-get install -y \ python3 \ python-dev \ python3-dev \ + python-tk \ + python3-tk \ python-opengl \ curl \ libsm6 \ diff --git a/docker/gpu/Dockerfile b/docker/gpu/Dockerfile index d621927d..58055cf3 100644 --- a/docker/gpu/Dockerfile +++ b/docker/gpu/Dockerfile @@ -12,6 +12,8 @@ RUN apt-get update && apt-get install -y \ python3 \ python-dev \ python3-dev \ + python-tk \ + python3-tk \ python-opengl \ curl \ libsm6 \