diff --git a/README.md b/README.md
index 869edc0..3c0a5a0 100644
--- a/README.md
+++ b/README.md
@@ -1,10 +1,12 @@
 ## Berkeley Autolab Perception Module
 
-[![Build Status](https://travis-ci.org/BerkeleyAutomation/perception.svg?branch=master)](https://travis-ci.org/BerkeleyAutomation/perception)
+[![Github Actions](https://github.com/BerkeleyAutomation/perception/actions/workflows/release.yml/badge.svg)](https://github.com/BerkeleyAutomation/perception/actions) [![PyPI version](https://badge.fury.io/py/autolab_perception.svg)](https://badge.fury.io/py/autolab_perception)
 
 This package provides a wide variety of useful tools for perception tasks.
 It directly depends on the [Berkeley Autolab Core
 module](https://www.github.com/BerkeleyAutomation/autolab_core), so be sure to install
 that first.
 View the install guide and API documentation for the perception module
-[here](https://BerkeleyAutomation.github.io/perception).
+[here](https://BerkeleyAutomation.github.io/perception). Dependencies for each driver are not automatically installed, so please install ROS or camera-specific drivers separately before using these wrappers.
+
+NOTE: As of May 4, 2021, this package no longer supports Python versions 3.5 or lower as these versions have reached EOL. In addition, many modules have been moved to `autolab_core` to reduce confusion. This repository now will contain sensor drivers and interfaces only. If you wish to use older Python versions or rely on the old modules, please use the 0.x.x series of tags.
\ No newline at end of file
diff --git a/docs/source/conf.py b/docs/source/conf.py
index 9358898..9679207 100644
--- a/docs/source/conf.py
+++ b/docs/source/conf.py
@@ -20,43 +20,41 @@
 # 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.abspath('../../'))
+sys.path.insert(0, os.path.abspath("../../"))
 
 # -- General configuration ------------------------------------------------
 
 # If your documentation needs a minimal Sphinx version, state it here.
-#needs_sphinx = '1.0'
+# needs_sphinx = '1.0'
 
 # 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'
-]
-autoclass_content = 'class'
-autodoc_member_order = 'bysource'
-autodoc_default_flags = ['members', 'show-inheritance']
+extensions = ["sphinx.ext.autodoc", "sphinxcontrib.napoleon"]
+autoclass_content = "class"
+autodoc_member_order = "bysource"
+autodoc_default_flags = ["members", "show-inheritance"]
 napoleon_include_special_with_doc = True
 napoleon_include_init_with_doc = True
 
 # Add any paths that contain templates here, relative to this directory.
-templates_path = ['_templates']
+templates_path = ["_templates"]
 
 # The suffix(es) of source filenames.
 # You can specify multiple suffix as a list of string:
 # source_suffix = ['.rst', '.md']
-source_suffix = '.rst'
+source_suffix = ".rst"
 
 # The encoding of source files.
-#source_encoding = 'utf-8-sig'
+# source_encoding = 'utf-8-sig'
 
 # The master toctree document.
-master_doc = 'index'
+master_doc = "index"
 
 # General information about the project.
-project = u'perception'
-copyright = u'2016, Jeff Mahler'
-author = u'Jeff Mahler'
+project = u"perception"
+copyright = u"2016, Jeff Mahler"
+author = u"Jeff Mahler"
 
 # The version info for the project you're documenting, acts as replacement for
 # |version| and |release|, also used in various other places throughout the
@@ -76,9 +74,9 @@
 
 # There are two options for replacing |today|: either, you set today to some
 # non-false value, then it is used:
-#today = ''
+# today = ''
 # Else, today_fmt is used as the format for a strftime call.
-#today_fmt = '%B %d, %Y'
+# today_fmt = '%B %d, %Y'
 
 # List of patterns, relative to source directory, that match files and
 # directories to ignore when looking for source files.
@@ -86,27 +84,27 @@
 
 # The reST default role (used for this markup: `text`) to use for all
 # documents.
-#default_role = None
+# default_role = None
 
 # If true, '()' will be appended to :func: etc. cross-reference text.
-#add_function_parentheses = True
+# add_function_parentheses = True
 
 # If true, the current module name will be prepended to all description
 # unit titles (such as .. function::).
-#add_module_names = True
+# add_module_names = True
 
 # If true, sectionauthor and moduleauthor directives will be shown in the
 # output. They are ignored by default.
-#show_authors = False
+# show_authors = False
 
 # The name of the Pygments (syntax highlighting) style to use.
-pygments_style = 'sphinx'
+pygments_style = "sphinx"
 
 # A list of ignored prefixes for module index sorting.
-#modindex_common_prefix = []
+# modindex_common_prefix = []
 
 # If true, keep warnings as "system message" paragraphs in the built documents.
-#keep_warnings = False
+# keep_warnings = False
 
 # If true, `todo` and `todoList` produce output, else they produce nothing.
 todo_include_todos = False
@@ -116,145 +114,147 @@
 
 # The theme to use for HTML and HTML Help pages.  See the documentation for
 # a list of builtin themes.
-#html_theme = 'alabaster'
-html_theme = 'sphinx_rtd_theme'
+# html_theme = 'alabaster'
+html_theme = "sphinx_rtd_theme"
 html_theme_path = [sphinx_rtd_theme.get_html_theme_path()]
 
 # Theme options are theme-specific and customize the look and feel of a theme
 # further.  For a list of options available for each theme, see the
 # documentation.
-#html_theme_options = {}
+# html_theme_options = {}
 
 # Add any paths that contain custom themes here, relative to this directory.
-#html_theme_path = []
+# html_theme_path = []
 
 # The name for this set of Sphinx documents.  If None, it defaults to
 # "<project> v<release> documentation".
-#html_title = None
+# html_title = None
 
 # A shorter title for the navigation bar.  Default is the same as html_title.
-#html_short_title = None
+# html_short_title = None
 
 # The name of an image file (relative to this directory) to place at the top
 # of the sidebar.
-#html_logo = None
+# html_logo = None
 
 # The name of an image file (relative to this directory) to use as a favicon of
 # the docs.  This file should be a Windows icon file (.ico) being 16x16 or 32x32
 # pixels large.
-#html_favicon = None
+# html_favicon = None
 
 # Add any paths that contain custom static files (such as style sheets) here,
 # relative to this directory. They are copied after the builtin static files,
 # so a file named "default.css" will overwrite the builtin "default.css".
-html_static_path = ['_static']
+html_static_path = ["_static"]
 
 # Add any extra paths that contain custom files (such as robots.txt or
 # .htaccess) here, relative to this directory. These files are copied
 # directly to the root of the documentation.
-#html_extra_path = []
+# html_extra_path = []
 
 # If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
 # using the given strftime format.
-#html_last_updated_fmt = '%b %d, %Y'
+# html_last_updated_fmt = '%b %d, %Y'
 
 # If true, SmartyPants will be used to convert quotes and dashes to
 # typographically correct entities.
-#html_use_smartypants = True
+# html_use_smartypants = True
 
 # Custom sidebar templates, maps document names to template names.
-#html_sidebars = {}
+# html_sidebars = {}
 
 # Additional templates that should be rendered to pages, maps page names to
 # template names.
-#html_additional_pages = {}
+# html_additional_pages = {}
 
 # If false, no module index is generated.
-#html_domain_indices = True
+# html_domain_indices = True
 
 # If false, no index is generated.
-#html_use_index = True
+# html_use_index = True
 
 # If true, the index is split into individual pages for each letter.
-#html_split_index = False
+# html_split_index = False
 
 # If true, links to the reST sources are added to the pages.
-#html_show_sourcelink = True
+# html_show_sourcelink = True
 
 # If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
-#html_show_sphinx = True
+# html_show_sphinx = True
 
 # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
-#html_show_copyright = True
+# html_show_copyright = True
 
 # If true, an OpenSearch description file will be output, and all pages will
 # contain a <link> tag referring to it.  The value of this option must be the
 # base URL from which the finished HTML is served.
-#html_use_opensearch = ''
+# html_use_opensearch = ''
 
 # This is the file name suffix for HTML files (e.g. ".xhtml").
-#html_file_suffix = None
+# html_file_suffix = None
 
 # Language to be used for generating the HTML full-text search index.
 # Sphinx supports the following languages:
 #   'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja'
 #   'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr'
-#html_search_language = 'en'
+# html_search_language = 'en'
 
 # A dictionary with options for the search language support, empty by default.
 # Now only 'ja' uses this config value
-#html_search_options = {'type': 'default'}
+# html_search_options = {'type': 'default'}
 
 # The name of a javascript file (relative to the configuration directory) that
 # implements a search results scorer. If empty, the default will be used.
-#html_search_scorer = 'scorer.js'
+# html_search_scorer = 'scorer.js'
 
 # Output file base name for HTML help builder.
-htmlhelp_basename = 'perceptiondoc'
+htmlhelp_basename = "perceptiondoc"
 
 # -- Options for LaTeX output ---------------------------------------------
 
 latex_elements = {
-# The paper size ('letterpaper' or 'a4paper').
-#'papersize': 'letterpaper',
-
-# The font size ('10pt', '11pt' or '12pt').
-#'pointsize': '10pt',
-
-# Additional stuff for the LaTeX preamble.
-#'preamble': '',
-
-# Latex figure (float) alignment
-#'figure_align': 'htbp',
+    # The paper size ('letterpaper' or 'a4paper').
+    #'papersize': 'letterpaper',
+    # The font size ('10pt', '11pt' or '12pt').
+    #'pointsize': '10pt',
+    # Additional stuff for the LaTeX preamble.
+    #'preamble': '',
+    # Latex figure (float) alignment
+    #'figure_align': 'htbp',
 }
 
 # Grouping the document tree into LaTeX files. List of tuples
 # (source start file, target name, title,
 #  author, documentclass [howto, manual, or own class]).
 latex_documents = [
-    (master_doc, 'perception.tex', u'perception Documentation',
-     u'Jeff Mahler', 'manual'),
+    (
+        master_doc,
+        "perception.tex",
+        u"perception Documentation",
+        u"Jeff Mahler",
+        "manual",
+    ),
 ]
 
 # The name of an image file (relative to this directory) to place at the top of
 # the title page.
-#latex_logo = None
+# latex_logo = None
 
 # For "manual" documents, if this is true, then toplevel headings are parts,
 # not chapters.
-#latex_use_parts = False
+# latex_use_parts = False
 
 # If true, show page references after internal links.
-#latex_show_pagerefs = False
+# latex_show_pagerefs = False
 
 # If true, show URL addresses after external links.
-#latex_show_urls = False
+# latex_show_urls = False
 
 # Documents to append as an appendix to all manuals.
-#latex_appendices = []
+# latex_appendices = []
 
 # If false, no module index is generated.
-#latex_domain_indices = True
+# latex_domain_indices = True
 
 
 # -- Options for manual page output ---------------------------------------
@@ -262,12 +262,11 @@
 # One entry per manual page. List of tuples
 # (source start file, name, description, authors, manual section).
 man_pages = [
-    (master_doc, 'perception', u'perception Documentation',
-     [author], 1)
+    (master_doc, "perception", u"perception Documentation", [author], 1)
 ]
 
 # If true, show URL addresses after external links.
-#man_show_urls = False
+# man_show_urls = False
 
 
 # -- Options for Texinfo output -------------------------------------------
@@ -276,19 +275,25 @@
 # (source start file, target name, title, author,
 #  dir menu entry, description, category)
 texinfo_documents = [
-    (master_doc, 'perception', u'perception Documentation',
-     author, 'perception', 'One line description of project.',
-     'Miscellaneous'),
+    (
+        master_doc,
+        "perception",
+        u"perception Documentation",
+        author,
+        "perception",
+        "One line description of project.",
+        "Miscellaneous",
+    ),
 ]
 
 # Documents to append as an appendix to all manuals.
-#texinfo_appendices = []
+# texinfo_appendices = []
 
 # If false, no module index is generated.
-#texinfo_domain_indices = True
+# texinfo_domain_indices = True
 
 # How to display URL addresses: 'footnote', 'no', or 'inline'.
-#texinfo_show_urls = 'footnote'
+# texinfo_show_urls = 'footnote'
 
 # If true, do not generate a @detailmenu in the "Top" node's menu.
-#texinfo_no_detailmenu = False
+# texinfo_no_detailmenu = False
diff --git a/package.xml b/package.xml
index 0872bf2..9830555 100644
--- a/package.xml
+++ b/package.xml
@@ -1,13 +1,11 @@
 <?xml version="1.0"?>
 <package>
   <name>perception</name>
-  <version>0.0.10</version>
-  <description>The perception package</description>
+  <version>1.0.0</version>
+  <description>Autolab Drivers package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="mdanielczuk@berkeley.edu">todo</maintainer>
+  <maintainer email="mdanielczuk@berkeley.edu">Mike Danielczuk</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
diff --git a/perception/version.py b/perception/version.py
index 9b36b86..5becc17 100644
--- a/perception/version.py
+++ b/perception/version.py
@@ -1 +1 @@
-__version__ = "0.0.10"
+__version__ = "1.0.0"
diff --git a/setup.py b/setup.py
index bcb8126..fe4ff42 100644
--- a/setup.py
+++ b/setup.py
@@ -2,57 +2,50 @@
 Setup of Berkeley AUTOLab Perception module Python codebase.
 Author: Jeff Mahler
 """
+import os
 from setuptools import setup
 
 requirements = [
-    'numpy',
-    'scipy',
-    'autolab_core',
-    'matplotlib',
-    'multiprocess',
-    'opencv-python',
-    'keras',
-    'cycler',
-    'Pillow',
-    'pyserial>=3.4',
-    'scikit-image',
-    'scikit-learn',
-    'scikit-video',
-    'ffmpeg-python'
+    "numpy",
+    "scipy",
+    "autolab_core",
+    "opencv-python",
+    "pyserial>=3.4",
+    "ffmpeg-python",
 ]
 
-exec(open('perception/version.py').read())
+# load __version__ without importing anything
+version_file = os.path.join(os.path.dirname(__file__), "perception/version.py")
+with open(version_file, "r") as f:
+    # use eval to get a clean string of version from file
+    __version__ = eval(f.read().strip().split("=")[-1])
 
-setup(name='autolab_perception',
-      version=__version__,
-      description='Perception utilities for the Berkeley AutoLab',
-      author='Jeff Mahler',
-      author_email='jmahler@berkeley.edu',
-      license = 'Apache Software License',
-      url = 'https://github.com/BerkeleyAutomation/perception',
-      keywords = 'robotics grasping vision perception',
-      classifiers = [
-          'Development Status :: 4 - Beta',
-          'License :: OSI Approved :: Apache Software License',
-          'Programming Language :: Python',
-          'Programming Language :: Python :: 2.7',
-          'Programming Language :: Python :: 3.5',
-          'Programming Language :: Python :: 3.6',
-          'Natural Language :: English',
-          'Topic :: Scientific/Engineering'
-      ],
-      packages=['perception'],
-      install_requires = requirements,
-      extras_require = { 'docs' : [
-                            'sphinx',
-                            'sphinxcontrib-napoleon',
-                            'sphinx_rtd_theme'
-                        ],
-                       'ros' : [
-                           'primesense',
-                           'rospkg',
-                           'catkin_pkg',
-                           'empy'
-                        ],
-    }
+setup(
+    name="autolab_perception",
+    version=__version__,
+    description="Perception utilities for the Berkeley AutoLab",
+    author="Jeff Mahler",
+    author_email="jmahler@berkeley.edu",
+    maintainer="Mike Danielczuk",
+    maintainer_email="mdanielczuk@berkeley.edu",
+    license="Apache Software License",
+    url="https://github.com/BerkeleyAutomation/perception",
+    keywords="robotics grasping vision perception",
+    classifiers=[
+        "Development Status :: 4 - Beta",
+        "License :: OSI Approved :: Apache Software License",
+        "Programming Language :: Python",
+        "Programming Language :: Python :: 3.6",
+        "Programming Language :: Python :: 3.7",
+        "Programming Language :: Python :: 3.8",
+        "Programming Language :: Python :: 3.9",
+        "Natural Language :: English",
+        "Topic :: Scientific/Engineering",
+    ],
+    packages=["perception"],
+    install_requires=requirements,
+    extras_require={
+        "docs": ["sphinx", "sphinxcontrib-napoleon", "sphinx_rtd_theme"],
+        "ros": ["primesense", "rospkg", "catkin_pkg", "empy"],
+    },
 )
diff --git a/tests/test_rgbd_sensors.py b/tests/test_rgbd_sensors.py
new file mode 100644
index 0000000..c0da8b5
--- /dev/null
+++ b/tests/test_rgbd_sensors.py
@@ -0,0 +1,85 @@
+import os
+import numpy as np
+import unittest
+from autolab_core import ColorImage, DepthImage, CameraIntrinsics
+from perception import RgbdSensorFactory
+
+IM_FILEROOT = "tests/data"
+
+
+class TestRgbdSensors(unittest.TestCase):
+    def test_virtual(self, height=100, width=100):
+        # Generate folder of color and depth images
+        if not os.path.exists(IM_FILEROOT):
+            os.makedirs(IM_FILEROOT)
+        cam_intr = CameraIntrinsics(
+            "a",
+            fx=0.0,
+            fy=0.0,
+            cx=0.0,
+            cy=0.0,
+            skew=0.0,
+            height=100,
+            width=100,
+        )
+        cam_intr.save(os.path.join(IM_FILEROOT, "a.intr"))
+        color_data = (255 * np.random.rand(10, height, width, 3)).astype(
+            np.uint8
+        )
+        depth_data = np.random.rand(10, height, width).astype(np.float32)
+        for i in range(10):
+            im = ColorImage(color_data[i], frame="a")
+            im.save(os.path.join(IM_FILEROOT, "color_{:d}.png".format(i)))
+
+            im = DepthImage(depth_data[i], frame="a")
+            im.save(os.path.join(IM_FILEROOT, "depth_{:d}.npy".format(i)))
+
+        # Create virtual camera
+        virtual_cam = RgbdSensorFactory.sensor(
+            "virtual", cfg={"image_dir": IM_FILEROOT, "frame": "a"}
+        )
+        self.assertTrue(
+            virtual_cam.path_to_images == IM_FILEROOT,
+            msg="img path changed after init",
+        )
+
+        # Start virtual camera and read frames
+        virtual_cam.start()
+        self.assertTrue(
+            virtual_cam.is_running, msg="camera not running after start"
+        )
+        for i in range(10):
+            color, depth = virtual_cam.frames()
+            self.assertTrue(
+                np.all(color.data == color_data[i]),
+                msg="color data for img {:d} changed".format(i),
+            )
+            self.assertTrue(
+                color.frame == virtual_cam.frame,
+                msg="frame mismatch between color im and camera",
+            )
+            self.assertTrue(
+                np.all(depth.data == depth_data[i]),
+                msg="depth data for img {:d} changed".format(i),
+            )
+            self.assertTrue(
+                depth.frame == virtual_cam.frame,
+                msg="frame mismatch between depth im and camera",
+            )
+
+        # Make sure camera is stopped
+        virtual_cam.stop()
+        self.assertFalse(
+            virtual_cam.is_running, msg="camera running after stop"
+        )
+
+        # Cleanup images
+        for i in range(10):
+            os.remove(os.path.join(IM_FILEROOT, "color_{:d}.png".format(i)))
+            os.remove(os.path.join(IM_FILEROOT, "depth_{:d}.npy".format(i)))
+        os.remove(os.path.join(IM_FILEROOT, "a.intr"))
+        os.rmdir(IM_FILEROOT)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/tools/capture_dataset.py b/tools/capture_dataset.py
index 0d3334b..e44b16c 100644
--- a/tools/capture_dataset.py
+++ b/tools/capture_dataset.py
@@ -16,28 +16,37 @@
 import matplotlib.pyplot as plt
 
 import autolab_core.utils as utils
-from autolab_core import Box, PointCloud, RigidTransform, TensorDataset, YamlConfig
-from autolab_core.constants import *
+from autolab_core import (
+    Box,
+    PointCloud,
+    RigidTransform,
+    TensorDataset,
+    YamlConfig,
+)
+from autolab_core.constants import TRAIN_ID, TEST_ID
 from meshrender import Scene, SceneObject, VirtualCamera, MaterialProperties
-from perception import RgbdSensorFactory, Image, RenderMode
+from perception import RgbdSensorFactory
 from visualization import Visualizer2D as vis2d
 
 GUI_PAUSE = 0.5
 
-def preprocess_images(raw_color_im,
-                      raw_depth_im,
-                      camera_intr,
-                      T_camera_world,
-                      workspace_box,
-                      workspace_im,
-                      image_proc_config):
-    """ Preprocess a set of color and depth images. """
+
+def preprocess_images(
+    raw_color_im,
+    raw_depth_im,
+    camera_intr,
+    T_camera_world,
+    workspace_box,
+    workspace_im,
+    image_proc_config,
+):
+    """Preprocess a set of color and depth images."""
     # read params
-    inpaint_rescale_factor = image_proc_config['inpaint_rescale_factor']
-    cluster = image_proc_config['cluster']
-    cluster_tolerance = image_proc_config['cluster_tolerance']
-    min_cluster_size = image_proc_config['min_cluster_size']
-    max_cluster_size = image_proc_config['max_cluster_size']
+    inpaint_rescale_factor = image_proc_config["inpaint_rescale_factor"]
+    cluster = image_proc_config["cluster"]
+    cluster_tolerance = image_proc_config["cluster_tolerance"]
+    min_cluster_size = image_proc_config["min_cluster_size"]
+    max_cluster_size = image_proc_config["max_cluster_size"]
 
     # deproject into 3D world coordinates
     point_cloud_cam = camera_intr.deproject(raw_depth_im)
@@ -51,27 +60,36 @@ def preprocess_images(raw_color_im,
 
     # mask out objects in the known workspace
     env_pixels = depth_im_seg.pixels_farther_than(workspace_im)
-    depth_im_seg._data[env_pixels[:,0], env_pixels[:,1]] = 0
+    depth_im_seg._data[env_pixels[:, 0], env_pixels[:, 1]] = 0
 
     # REMOVE NOISE
     # clip low points
-    low_indices = np.where(point_cloud_world.data[2,:] < workspace_box.min_pt[2])[0] 
-    point_cloud_world.data[2,low_indices] = workspace_box.min_pt[2]
-            
+    low_indices = np.where(
+        point_cloud_world.data[2, :] < workspace_box.min_pt[2]
+    )[0]
+    point_cloud_world.data[2, low_indices] = workspace_box.min_pt[2]
+
     # clip high points
-    high_indices = np.where(point_cloud_world.data[2,:] > workspace_box.max_pt[2])[0] 
-    point_cloud_world.data[2,high_indices] = workspace_box.max_pt[2]
+    high_indices = np.where(
+        point_cloud_world.data[2, :] > workspace_box.max_pt[2]
+    )[0]
+    point_cloud_world.data[2, high_indices] = workspace_box.max_pt[2]
 
     # segment out the region in the workspace (including the table)
-    workspace_point_cloud_world, valid_indices = point_cloud_world.box_mask(workspace_box)
-    invalid_indices = np.setdiff1d(np.arange(point_cloud_world.num_points),
-                                   valid_indices)
+    workspace_point_cloud_world, valid_indices = point_cloud_world.box_mask(
+        workspace_box
+    )
+    invalid_indices = np.setdiff1d(
+        np.arange(point_cloud_world.num_points), valid_indices
+    )
 
     if cluster:
         # create new cloud
-        pcl_cloud = pcl.PointCloud(workspace_point_cloud_world.data.T.astype(np.float32))
+        pcl_cloud = pcl.PointCloud(
+            workspace_point_cloud_world.data.T.astype(np.float32)
+        )
         tree = pcl_cloud.make_kdtree()
-    
+
         # find large clusters (likely to be real objects instead of noise)
         ec = pcl_cloud.make_EuclideanClusterExtraction()
         ec.set_ClusterTolerance(cluster_tolerance)
@@ -79,55 +97,70 @@ def preprocess_images(raw_color_im,
         ec.set_MaxClusterSize(max_cluster_size)
         ec.set_SearchMethod(tree)
         cluster_indices = ec.Extract()
-        num_clusters = len(cluster_indices)
 
         # read out all points in large clusters
-        filtered_points = np.zeros([3,workspace_point_cloud_world.num_points])
+        filtered_points = np.zeros([3, workspace_point_cloud_world.num_points])
         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]
 
-            filtered_points[:,cur_i:cur_i+num_points] = points.copy()
+            filtered_points[:, cur_i : cur_i + num_points] = points.copy()
             cur_i = cur_i + num_points
 
         # reconstruct the point cloud
-        all_points = np.c_[filtered_points[:,:cur_i], point_cloud_world.data[:,invalid_indices]]
+        all_points = np.c_[
+            filtered_points[:, :cur_i],
+            point_cloud_world.data[:, invalid_indices],
+        ]
     else:
         all_points = point_cloud_world.data
-    filtered_point_cloud_world = PointCloud(all_points,
-                                            frame='world')  
+    filtered_point_cloud_world = PointCloud(all_points, frame="world")
 
     # compute the filtered depth image
-    filtered_point_cloud_cam = T_camera_world.inverse() * filtered_point_cloud_world
-    depth_im = camera_intr.project_to_image(filtered_point_cloud_cam)    
+    filtered_point_cloud_cam = (
+        T_camera_world.inverse() * filtered_point_cloud_world
+    )
+    depth_im = camera_intr.project_to_image(filtered_point_cloud_cam)
 
     # form segmask
     segmask = depth_im_seg.to_binary()
     valid_px_segmask = depth_im.invalid_pixel_mask().inverse()
     segmask = segmask.mask_binary(valid_px_segmask)
-    
+
     # inpaint
     color_im = raw_color_im.inpaint(rescale_factor=inpaint_rescale_factor)
-    depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)    
-    return color_im, depth_im, segmask    
-    
-if __name__ == '__main__':
+    depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor)
+    return color_im, depth_im, segmask
+
+
+if __name__ == "__main__":
     # set up logger
     logging.getLogger().setLevel(logging.INFO)
-    rospy.init_node('capture_dataset', anonymous=True)
+    rospy.init_node("capture_dataset", anonymous=True)
     logging.getLogger().addHandler(rl.RosStreamHandler())
 
     # parse args
-    parser = argparse.ArgumentParser(description='Capture a dataset of RGB-D images from a set of sensors')
-    parser.add_argument('output_dir', type=str, help='directory to save output')
-    parser.add_argument('num_images', type=int, help='number of images to capture')
-    parser.add_argument('--config_filename', type=str, default=None, help='path to configuration file to use')
+    parser = argparse.ArgumentParser(
+        description="Capture a dataset of RGB-D images from a set of sensors"
+    )
+    parser.add_argument(
+        "output_dir", type=str, help="directory to save output"
+    )
+    parser.add_argument(
+        "num_images", type=int, help="number of images to capture"
+    )
+    parser.add_argument(
+        "--config_filename",
+        type=str,
+        default=None,
+        help="path to configuration file to use",
+    )
     args = parser.parse_args()
     output_dir = args.output_dir
     num_images = args.num_images
@@ -136,28 +169,29 @@ def preprocess_images(raw_color_im,
     # make output directory
     if not os.path.exists(output_dir):
         os.mkdir(output_dir)
-    
+
     # fix config
     if config_filename is None:
-        config_filename = os.path.join(os.path.dirname(os.path.realpath(__file__)),
-                                       '..',
-                                       'cfg/tools/capture_dataset.yaml')
+        config_filename = os.path.join(
+            os.path.dirname(os.path.realpath(__file__)),
+            "..",
+            "cfg/tools/capture_dataset.yaml",
+        )
 
     # turn relative paths absolute
     if not os.path.isabs(config_filename):
         config_filename = os.path.join(os.getcwd(), config_filename)
 
-    
     # read config
     config = YamlConfig(config_filename)
-    dataset_config = config['dataset']
-    sensor_configs = config['sensors']
-    workspace_config = config['workspace']
-    image_proc_config = config['image_proc']
+    dataset_config = config["dataset"]
+    sensor_configs = config["sensors"]
+    workspace_config = config["workspace"]
+    image_proc_config = config["image_proc"]
 
     # read objects
-    train_pct = config['train_pct']
-    objects = config['objects']
+    train_pct = config["train_pct"]
+    objects = config["objects"]
     num_objects = len(objects)
     num_train = int(np.ceil(train_pct * num_objects))
     num_test = num_objects - num_train
@@ -172,42 +206,42 @@ def preprocess_images(raw_color_im,
     train_image_indices = all_image_indices[:num_train_images]
 
     # set random variable for the number of objects
-    mean_num_objects = config['mean_num_objects']
-    min_num_objects = config['min_num_objects']
-    max_num_objects = config['max_num_objects']
-    num_objs_rv = ss.poisson(mean_num_objects-1)
-    im_rescale_factor = image_proc_config['im_rescale_factor']
-
-    save_raw = config['save_raw']
-    vis = config['vis']
-    
+    mean_num_objects = config["mean_num_objects"]
+    min_num_objects = config["min_num_objects"]
+    max_num_objects = config["max_num_objects"]
+    num_objs_rv = ss.poisson(mean_num_objects - 1)
+    im_rescale_factor = image_proc_config["im_rescale_factor"]
+
+    save_raw = config["save_raw"]
+    vis = config["vis"]
+
     # open gui
-    gui = plt.figure(0, figsize=(8,8))
+    gui = plt.figure(0, figsize=(8, 8))
     plt.ion()
-    plt.title('INITIALIZING')
-    plt.imshow(np.zeros([100,100]),
-               cmap=plt.cm.gray_r)
-    plt.axis('off')
+    plt.title("INITIALIZING")
+    plt.imshow(np.zeros([100, 100]), cmap=plt.cm.gray_r)
+    plt.axis("off")
     plt.draw()
     plt.pause(GUI_PAUSE)
-    
+
     # read workspace bounds
-    workspace_box = Box(np.array(workspace_config['min_pt']),
-                        np.array(workspace_config['max_pt']),
-                        frame='world')
-    
+    workspace_box = Box(
+        np.array(workspace_config["min_pt"]),
+        np.array(workspace_config["max_pt"]),
+        frame="world",
+    )
+
     # read workspace objects
     workspace_objects = {}
-    for obj_key, obj_config in workspace_config['objects'].iteritems():
-        mesh_filename = obj_config['mesh_filename']
-        pose_filename = obj_config['pose_filename']
+    for obj_key, obj_config in workspace_config["objects"].iteritems():
+        mesh_filename = obj_config["mesh_filename"]
+        pose_filename = obj_config["pose_filename"]
         obj_mesh = trimesh.load_mesh(mesh_filename)
         obj_pose = RigidTransform.load(pose_filename)
-        obj_mat_props = MaterialProperties(smooth=True,
-                                           wireframe=False)
+        obj_mat_props = MaterialProperties(smooth=True, wireframe=False)
         scene_obj = SceneObject(obj_mesh, obj_pose, obj_mat_props)
         workspace_objects[obj_key] = scene_obj
-        
+
     # setup each sensor
     datasets = {}
     sensors = {}
@@ -216,64 +250,71 @@ def preprocess_images(raw_color_im,
     workspace_ims = {}
     for sensor_name, sensor_config in sensor_configs.iteritems():
         # read params
-        sensor_type = sensor_config['type']
-        sensor_frame = sensor_config['frame']
-        
+        sensor_type = sensor_config["type"]
+        sensor_frame = sensor_config["frame"]
+
         # read camera calib
-        tf_filename = '%s_to_world.tf' %(sensor_frame)
-        T_camera_world = RigidTransform.load(os.path.join(sensor_config['calib_dir'], sensor_frame, tf_filename))
+        tf_filename = "%s_to_world.tf" % (sensor_frame)
+        T_camera_world = RigidTransform.load(
+            os.path.join(sensor_config["calib_dir"], sensor_frame, tf_filename)
+        )
         sensor_poses[sensor_name] = T_camera_world
-        
+
         # setup sensor
         sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
         sensors[sensor_name] = sensor
-        
+
         # start the sensor
         sensor.start()
         camera_intr = sensor.ir_intrinsics
         camera_intr = camera_intr.resize(im_rescale_factor)
-        camera_intrs[sensor_name] = camera_intr        
-        
+        camera_intrs[sensor_name] = camera_intr
+
         # render image of static workspace
         scene = Scene()
         camera = VirtualCamera(camera_intr, T_camera_world)
         scene.camera = camera
         for obj_key, scene_obj in workspace_objects.iteritems():
             scene.add_object(obj_key, scene_obj)
-        workspace_ims[sensor_name] = scene.wrapped_render([RenderMode.DEPTH])[0]
+        workspace_ims[sensor_name] = scene.wrapped_render(["depth"])[0]
 
         # fix dataset config
-        dataset_config['fields']['raw_color_ims']['height'] = camera_intr.height
-        dataset_config['fields']['raw_color_ims']['width'] = camera_intr.width
-        dataset_config['fields']['raw_depth_ims']['height'] = camera_intr.height
-        dataset_config['fields']['raw_depth_ims']['width'] = camera_intr.width 
-        dataset_config['fields']['color_ims']['height'] = camera_intr.height
-        dataset_config['fields']['color_ims']['width'] = camera_intr.width 
-        dataset_config['fields']['depth_ims']['height'] = camera_intr.height
-        dataset_config['fields']['depth_ims']['width'] = camera_intr.width 
-        dataset_config['fields']['segmasks']['height'] = camera_intr.height
-        dataset_config['fields']['segmasks']['width'] = camera_intr.width 
-       
+        dataset_config["fields"]["raw_color_ims"][
+            "height"
+        ] = camera_intr.height
+        dataset_config["fields"]["raw_color_ims"]["width"] = camera_intr.width
+        dataset_config["fields"]["raw_depth_ims"][
+            "height"
+        ] = camera_intr.height
+        dataset_config["fields"]["raw_depth_ims"]["width"] = camera_intr.width
+        dataset_config["fields"]["color_ims"]["height"] = camera_intr.height
+        dataset_config["fields"]["color_ims"]["width"] = camera_intr.width
+        dataset_config["fields"]["depth_ims"]["height"] = camera_intr.height
+        dataset_config["fields"]["depth_ims"]["width"] = camera_intr.width
+        dataset_config["fields"]["segmasks"]["height"] = camera_intr.height
+        dataset_config["fields"]["segmasks"]["width"] = camera_intr.width
+
         # open dataset
         sensor_dataset_filename = os.path.join(output_dir, sensor_name)
-        datasets[sensor_name] = TensorDataset(sensor_dataset_filename,
-                                              dataset_config)        
+        datasets[sensor_name] = TensorDataset(
+            sensor_dataset_filename, dataset_config
+        )
 
         # save raw
         if save_raw:
             sensor_dir = os.path.join(output_dir, sensor_name)
-            raw_dir = os.path.join(sensor_dir, 'raw')
+            raw_dir = os.path.join(sensor_dir, "raw")
             if not os.path.exists(raw_dir):
                 os.mkdir(raw_dir)
 
-            camera_intr_filename = os.path.join(raw_dir, 'camera_intr.intr')
+            camera_intr_filename = os.path.join(raw_dir, "camera_intr.intr")
             camera_intr.save(camera_intr_filename)
-            camera_pose_filename = os.path.join(raw_dir, 'T_camera_world.tf')
+            camera_pose_filename = os.path.join(raw_dir, "T_camera_world.tf")
             T_camera_world.save(camera_pose_filename)
 
     # collect K images
     for k in range(num_images):
-        logging.info('Test case %d of %d' %(k, num_images))
+        logging.info("Test case %d of %d" % (k, num_images))
 
         # set test case
         train = 0
@@ -282,22 +323,36 @@ def preprocess_images(raw_color_im,
             train = 1
             split = TRAIN_ID
         if train:
-            num_objects = min(max(num_objs_rv.rvs(size=1)[0] + 1, min_num_objects), num_train)
-            obj_names = [objects[i] for i in np.random.choice(train_indices, size=num_objects, replace=False)]
+            num_objects = min(
+                max(num_objs_rv.rvs(size=1)[0] + 1, min_num_objects), num_train
+            )
+            obj_names = [
+                objects[i]
+                for i in np.random.choice(
+                    train_indices, size=num_objects, replace=False
+                )
+            ]
         else:
-            num_objects = min(max(num_objs_rv.rvs(size=1)[0] + 1, min_num_objects), num_test)
-            obj_names = [objects[i] for i in np.random.choice(test_indices, size=num_objects, replace=False)]
-            
+            num_objects = min(
+                max(num_objs_rv.rvs(size=1)[0] + 1, min_num_objects), num_test
+            )
+            obj_names = [
+                objects[i]
+                for i in np.random.choice(
+                    test_indices, size=num_objects, replace=False
+                )
+            ]
+
         # get human consent
-        message = 'Please place %d objects:\n' %(num_objects)
+        message = "Please place %d objects:\n" % (num_objects)
         for name in obj_names:
-            message += '\t{}\n'.format(name)
-        message += 'Hit ENTER when finished.'
+            message += "\t{}\n".format(name)
+        message += "Hit ENTER when finished."
         utils.keyboard_input(message=message)
 
         # capture
         for sensor_name, sensor in sensors.iteritems():
-            logging.info('Capturing images from sensor %s' %(sensor_name))
+            logging.info("Capturing images from sensor %s" % (sensor_name))
 
             # read pose and intrinsics
             sensor_pose = sensor_poses[sensor_name]
@@ -306,75 +361,84 @@ def preprocess_images(raw_color_im,
             dataset = datasets[sensor_name]
             T_camera_world = sensor_pose
             datapoint = dataset.datapoint_template
-            
+
             # read raw images
             raw_color_im, raw_depth_im, _ = sensor.frames()
 
             # resize
             raw_color_im = raw_color_im.resize(im_rescale_factor)
-            raw_depth_im = raw_depth_im.resize(im_rescale_factor,
-                                               interp='nearest')
+            raw_depth_im = raw_depth_im.resize(
+                im_rescale_factor, interp="nearest"
+            )
 
             # preprocess
-            color_im, depth_im, segmask = preprocess_images(raw_color_im,
-                                                            raw_depth_im,
-                                                            camera_intr,
-                                                            T_camera_world,
-                                                            workspace_box,
-                                                            workspace_im,
-                                                            image_proc_config)
-            
+            color_im, depth_im, segmask = preprocess_images(
+                raw_color_im,
+                raw_depth_im,
+                camera_intr,
+                T_camera_world,
+                workspace_box,
+                workspace_im,
+                image_proc_config,
+            )
+
             # visualize
             if vis:
                 gui = plt.figure(0)
                 plt.clf()
-                vis2d.subplot(2,3,1)
+                vis2d.subplot(2, 3, 1)
                 vis2d.imshow(raw_color_im)
-                vis2d.title('RAW COLOR')
-                vis2d.subplot(2,3,2)
+                vis2d.title("RAW COLOR")
+                vis2d.subplot(2, 3, 2)
                 vis2d.imshow(raw_depth_im)
-                vis2d.title('RAW DEPTH')
-                vis2d.subplot(2,3,4)
+                vis2d.title("RAW DEPTH")
+                vis2d.subplot(2, 3, 4)
                 vis2d.imshow(color_im)
-                vis2d.title('COLOR')
-                vis2d.subplot(2,3,5)
+                vis2d.title("COLOR")
+                vis2d.subplot(2, 3, 5)
                 vis2d.imshow(depth_im)
-                vis2d.title('DEPTH')
-                vis2d.subplot(2,3,6)
+                vis2d.title("DEPTH")
+                vis2d.subplot(2, 3, 6)
                 vis2d.imshow(segmask)
-                vis2d.title('SEGMASK')
+                vis2d.title("SEGMASK")
                 plt.draw()
                 plt.pause(GUI_PAUSE)
-            
+
             # save data
-            datapoint['split'] = split
-            datapoint['camera_intrs'] = camera_intr.vec
-            datapoint['camera_poses'] = sensor_pose.vec
-            datapoint['raw_color_ims'] = raw_color_im.raw_data
-            datapoint['raw_depth_ims'] = raw_depth_im.raw_data
-            datapoint['color_ims'] = color_im.raw_data
-            datapoint['depth_ims'] = depth_im.raw_data
-            datapoint['segmasks'] = segmask.raw_data
+            datapoint["split"] = split
+            datapoint["camera_intrs"] = camera_intr.vec
+            datapoint["camera_poses"] = sensor_pose.vec
+            datapoint["raw_color_ims"] = raw_color_im.raw_data
+            datapoint["raw_depth_ims"] = raw_depth_im.raw_data
+            datapoint["color_ims"] = color_im.raw_data
+            datapoint["depth_ims"] = depth_im.raw_data
+            datapoint["segmasks"] = segmask.raw_data
             dataset.add(datapoint)
 
             # save raw data
             if save_raw:
                 sensor_dir = os.path.join(output_dir, sensor_name)
-                raw_dir = os.path.join(sensor_dir, 'raw')
+                raw_dir = os.path.join(sensor_dir, "raw")
 
-                raw_color_im_filename = os.path.join(raw_dir, 'raw_color_%d.png' %(k))
+                raw_color_im_filename = os.path.join(
+                    raw_dir, "raw_color_%d.png" % (k)
+                )
                 raw_color_im.save(raw_color_im_filename)
-                color_im_filename = os.path.join(raw_dir, 'color_%d.png' %(k))
+                color_im_filename = os.path.join(raw_dir, "color_%d.png" % (k))
                 color_im.save(color_im_filename)
-                
-                raw_depth_im_filename = os.path.join(raw_dir, 'raw_depth_%d.npy' %(k))
+
+                raw_depth_im_filename = os.path.join(
+                    raw_dir, "raw_depth_%d.npy" % (k)
+                )
                 raw_depth_im.save(raw_depth_im_filename)
-                depth_im_filename = os.path.join(raw_dir, 'depth_%d.npy' %(k))
+                depth_im_filename = os.path.join(raw_dir, "depth_%d.npy" % (k))
                 depth_im.save(depth_im_filename)
 
-                segmask_filename = os.path.join(raw_dir, 'segmask_%d.png' %(k))
+                segmask_filename = os.path.join(
+                    raw_dir, "segmask_%d.png" % (k)
+                )
                 segmask.save(segmask_filename)
-                
+
     # stop all sensors
     for sensor_name, sensor in sensors.iteritems():
         datasets[sensor_name].flush()
diff --git a/tools/capture_test_images.py b/tools/capture_test_images.py
index 0a3e18a..d025f19 100644
--- a/tools/capture_test_images.py
+++ b/tools/capture_test_images.py
@@ -6,29 +6,36 @@
 import argparse
 import numpy as np
 import os
-
 import rospy
-import matplotlib.pyplot as plt
 
 from autolab_core import RigidTransform, Box, YamlConfig, Logger
 import autolab_core.utils as utils
-from perception import RgbdSensorFactory, Image
+from perception import RgbdSensorFactory
 
 # set up logger
-logger = Logger.get_logger('tools/capture_test_images.py')
+logger = Logger.get_logger("tools/capture_test_images.py")
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     # parse args
-    parser = argparse.ArgumentParser(description='Capture a set of RGB-D images from a set of sensors')
-    parser.add_argument('output_dir', type=str, help='path to save captured images')
-    parser.add_argument('--config_filename', type=str, default='cfg/tools/capture_test_images.yaml', help='path to configuration file to use')
+    parser = argparse.ArgumentParser(
+        description="Capture a set of RGB-D images from a set of sensors"
+    )
+    parser.add_argument(
+        "output_dir", type=str, help="path to save captured images"
+    )
+    parser.add_argument(
+        "--config_filename",
+        type=str,
+        default="cfg/tools/capture_test_images.yaml",
+        help="path to configuration file to use",
+    )
     args = parser.parse_args()
     config_filename = args.config_filename
     output_dir = args.output_dir
 
     # read config
     config = YamlConfig(config_filename)
-    vis = config['vis']
+    vis = config["vis"]
 
     # make output dir if needed
     if not os.path.exists(output_dir):
@@ -36,33 +43,39 @@
 
     # read rescale factor
     rescale_factor = 1.0
-    if 'rescale_factor' in config.keys():
-        rescale_factor = config['rescale_factor']
+    if "rescale_factor" in config.keys():
+        rescale_factor = config["rescale_factor"]
 
     # read workspace bounds
     workspace = None
-    if 'workspace' in config.keys():
-        workspace = Box(np.array(config['workspace']['min_pt']),
-                        np.array(config['workspace']['max_pt']),
-                        frame='world')
-        
+    if "workspace" in config.keys():
+        workspace = Box(
+            np.array(config["workspace"]["min_pt"]),
+            np.array(config["workspace"]["max_pt"]),
+            frame="world",
+        )
+
     # init ros node
-    rospy.init_node('capture_test_images') #NOTE: this is required by the camera sensor classes
+    rospy.init_node(
+        "capture_test_images"
+    )  # NOTE: this is required by the camera sensor classes
     Logger.reconfigure_root()
 
-    for sensor_name, sensor_config in config['sensors'].iteritems():
-        logger.info('Capturing images from sensor %s' %(sensor_name))
+    for sensor_name, sensor_config in config["sensors"].iteritems():
+        logger.info("Capturing images from sensor %s" % (sensor_name))
         save_dir = os.path.join(output_dir, sensor_name)
         if not os.path.exists(save_dir):
-            os.mkdir(save_dir)        
-        
+            os.mkdir(save_dir)
+
         # read params
-        sensor_type = sensor_config['type']
-        sensor_frame = sensor_config['frame']
-        
+        sensor_type = sensor_config["type"]
+        sensor_frame = sensor_config["frame"]
+
         # 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)
+        )
         T_camera_world.save(os.path.join(save_dir, tf_filename))
 
         # setup sensor
@@ -71,14 +84,14 @@
         # start the sensor
         sensor.start()
         camera_intr = sensor.ir_intrinsics
-        camera_intr.save(os.path.join(save_dir, '%s.intr' %(sensor.frame)))
+        camera_intr.save(os.path.join(save_dir, "%s.intr" % (sensor.frame)))
 
         # get raw images
-        for i in range(sensor_config['num_images']):
-            logger.info('Capturing image %d' %(i))
-            message = 'Hit ENTER when ready.'
+        for i in range(sensor_config["num_images"]):
+            logger.info("Capturing image %d" % (i))
+            message = "Hit ENTER when ready."
             utils.keyboard_input(message=message)
-            
+
             # read images
             color, depth, ir = sensor.frames()
 
@@ -88,44 +101,51 @@
                 point_cloud_cam = camera_intr.deproject(depth)
                 point_cloud_cam.remove_zero_points()
                 point_cloud_world = T_camera_world * point_cloud_cam
-                
+
                 # segment out the region in the workspace (objects only)
-                seg_point_cloud_world, _ = point_cloud_world.box_mask(workspace)
+                seg_point_cloud_world, _ = point_cloud_world.box_mask(
+                    workspace
+                )
 
                 # compute the segmask for points above the box
-                seg_point_cloud_cam = T_camera_world.inverse() * seg_point_cloud_world
-                depth_im_seg = camera_intr.project_to_image(seg_point_cloud_cam)
+                seg_point_cloud_cam = (
+                    T_camera_world.inverse() * seg_point_cloud_world
+                )
+                depth_im_seg = camera_intr.project_to_image(
+                    seg_point_cloud_cam
+                )
                 segmask = depth_im_seg.to_binary()
 
                 # rescale segmask
                 if rescale_factor != 1.0:
-                    segmask = segmask.resize(rescale_factor, interp='nearest')
-                
+                    segmask = segmask.resize(rescale_factor, interp="nearest")
+
                 # save segmask
-                segmask.save(os.path.join(save_dir, 'segmask_%d.png' %(i)))
+                segmask.save(os.path.join(save_dir, "segmask_%d.png" % (i)))
 
             # rescale images
             if rescale_factor != 1.0:
                 color = color.resize(rescale_factor)
-                depth = depth.resize(rescale_factor, interp='nearest')
-            
+                depth = depth.resize(rescale_factor, interp="nearest")
+
             # save images
-            color.save(os.path.join(save_dir, 'color_%d.png' %(i)))
-            depth.save(os.path.join(save_dir, 'depth_%d.npy' %(i)))
+            color.save(os.path.join(save_dir, "color_%d.png" % (i)))
+            depth.save(os.path.join(save_dir, "depth_%d.npy" % (i)))
             if ir is not None:
-                ir.save(os.path.join(save_dir, 'ir_%d.npy' %(i)))
-                
+                ir.save(os.path.join(save_dir, "ir_%d.npy" % (i)))
+
             if vis:
                 from visualization import Visualizer2D as vis2d
+
                 num_plots = 3 if workspace is not None else 2
                 vis2d.figure()
-                vis2d.subplot(1,num_plots,1)
+                vis2d.subplot(1, num_plots, 1)
                 vis2d.imshow(color)
-                vis2d.subplot(1,num_plots,2)
+                vis2d.subplot(1, num_plots, 2)
                 vis2d.imshow(depth)
                 if workspace is not None:
-                    vis2d.subplot(1,num_plots,3)
+                    vis2d.subplot(1, num_plots, 3)
                     vis2d.imshow(segmask)
                 vis2d.show()
-                
+
         sensor.stop()
diff --git a/tools/colorize_phoxi.py b/tools/colorize_phoxi.py
index 874f8f3..1f05757 100644
--- a/tools/colorize_phoxi.py
+++ b/tools/colorize_phoxi.py
@@ -1,43 +1,46 @@
 """
 Script to register a webcam to the Photoneo PhoXi
 Author: Matt Matl
-""" 
+"""
 import argparse
 import logging
-import numpy as np
-import os
-import plyfile
-import time
 
 import rospy
 import rosgraph.roslogging as rl
 
-from autolab_core import YamlConfig, RigidTransform, PointCloud
-from perception import RgbdSensorFactory, ColorImage
+from autolab_core import YamlConfig
+from perception import RgbdSensorFactory
 
-from visualization import Visualizer2D as vis2d
 from visualization import Visualizer3D as vis3d
 
+
 def main():
     logging.getLogger().setLevel(logging.INFO)
 
     # parse args
-    parser = argparse.ArgumentParser(description='Register a webcam to the Photoneo PhoXi')
-    parser.add_argument('--config_filename', type=str, default='cfg/tools/colorize_phoxi.yaml', help='filename of a YAML configuration for registration')
+    parser = argparse.ArgumentParser(
+        description="Register a webcam to the Photoneo PhoXi"
+    )
+    parser.add_argument(
+        "--config_filename",
+        type=str,
+        default="cfg/tools/colorize_phoxi.yaml",
+        help="filename of a YAML configuration for registration",
+    )
     args = parser.parse_args()
     config_filename = args.config_filename
     config = YamlConfig(config_filename)
 
-    sensor_data = config['sensors']
-    phoxi_config = sensor_data['phoxi']
-    phoxi_config['frame'] = 'phoxi'
+    sensor_data = config["sensors"]
+    phoxi_config = sensor_data["phoxi"]
+    phoxi_config["frame"] = "phoxi"
 
     # Initialize ROS node
-    rospy.init_node('colorize_phoxi', anonymous=True)
+    rospy.init_node("colorize_phoxi", anonymous=True)
     logging.getLogger().addHandler(rl.RosStreamHandler())
 
     # Get PhoXi sensor set up
-    phoxi = RgbdSensorFactory.sensor(phoxi_config['type'], phoxi_config)
+    phoxi = RgbdSensorFactory.sensor(phoxi_config["type"], phoxi_config)
     phoxi.start()
 
     # Capture PhoXi and webcam images
@@ -51,20 +54,39 @@ def main():
     # vis2d.show()
 
     phoxi_pc = phoxi.ir_intrinsics.deproject(phoxi_depth_im)
-    colors = phoxi_color_im.data.reshape((phoxi_color_im.shape[0] * phoxi_color_im.shape[1], phoxi_color_im.shape[2])) / 255.0
+    colors = (
+        phoxi_color_im.data.reshape(
+            (
+                phoxi_color_im.shape[0] * phoxi_color_im.shape[1],
+                phoxi_color_im.shape[2],
+            )
+        )
+        / 255.0
+    )
     vis3d.figure()
     vis3d.points(phoxi_pc.data.T[::3], color=colors[::3], scale=0.001)
     vis3d.show()
 
     # Export to PLY file
     vertices = phoxi.ir_intrinsics.deproject(phoxi_depth_im).data.T
-    colors = phoxi_color_im.data.reshape(phoxi_color_im.data.shape[0] * phoxi_color_im.data.shape[1], phoxi_color_im.data.shape[2])
-    f = open('pcloud.ply', 'w')
-    f.write('ply\nformat ascii 1.0\nelement vertex {}\nproperty float x\nproperty float y\nproperty float z\nproperty uchar red\n'.format(len(vertices)) +
-            'property uchar green\nproperty uchar blue\nend_header\n')
-    for v, c in zip(vertices,colors):
-        f.write('{} {} {} {} {} {}\n'.format(v[0], v[1], v[2], c[0], c[1], c[2]))
+    colors = phoxi_color_im.data.reshape(
+        phoxi_color_im.data.shape[0] * phoxi_color_im.data.shape[1],
+        phoxi_color_im.data.shape[2],
+    )
+    f = open("pcloud.ply", "w")
+    f.write(
+        "ply\nformat ascii 1.0\nelement vertex {}\nproperty float x\n"
+        "property float y\nproperty float z\nproperty uchar red\n".format(
+            len(vertices)
+        )
+        + "property uchar green\nproperty uchar blue\nend_header\n"
+    )
+    for v, c in zip(vertices, colors):
+        f.write(
+            "{} {} {} {} {} {}\n".format(v[0], v[1], v[2], c[0], c[1], c[2])
+        )
     f.close()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()
diff --git a/tools/filter_images.py b/tools/filter_images.py
index a79f0de..6cf44e8 100644
--- a/tools/filter_images.py
+++ b/tools/filter_images.py
@@ -9,12 +9,10 @@
 import numpy as np
 import pcl
 import os
-import rospy
-import sys
 import time
 
 from autolab_core import Box, PointCloud, RigidTransform
-from perception import VirtualSensor
+from perception.virtual_camera_sensor import VirtualSensor
 from visualization import Visualizer2D as vis2d
 from visualization import Visualizer3D as vis3d
 
@@ -27,14 +25,16 @@
 vis_final_clouds = False
 vis_final_images = True
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     # set logging
     logging.getLogger().setLevel(logging.INFO)
 
     # parse args
-    parser = argparse.ArgumentParser(description='Filter a set of images')
-    parser.add_argument('image_dir', type=str, help='location to read the images from')
-    parser.add_argument('frame', type=str, help='frame of images')
+    parser = argparse.ArgumentParser(description="Filter a set of images")
+    parser.add_argument(
+        "image_dir", type=str, help="location to read the images from"
+    )
+    parser.add_argument("frame", type=str, help="frame of images")
     args = parser.parse_args()
     image_dir = args.image_dir
     frame = args.frame
@@ -45,8 +45,10 @@
     camera_intr = sensor.ir_intrinsics
 
     # read tf
-    T_camera_world = RigidTransform.load(os.path.join(image_dir, '%s_to_world.tf' %(frame)))
-    
+    T_camera_world = RigidTransform.load(
+        os.path.join(image_dir, "%s_to_world.tf" % (frame))
+    )
+
     # read images
     color_im, depth_im, _ = sensor.frames()
 
@@ -56,8 +58,8 @@
 
     # timing
     filter_start = time.time()
-    
-    small_depth_im = depth_im.resize(rescale_factor, interp='nearest')
+
+    small_depth_im = depth_im.resize(rescale_factor, interp="nearest")
     small_camera_intr = camera_intr.resize(rescale_factor)
 
     # convert to point cloud in world coords
@@ -67,55 +69,57 @@
     point_cloud_world = T_camera_world * point_cloud_cam
 
     point_cloud_filtered = copy.deepcopy(point_cloud_world)
-    logging.info('Deproject took %.3f sec' %(time.time()-deproject_start))
-    
+    logging.info("Deproject took %.3f sec" % (time.time() - deproject_start))
+
     # filter low
     clip_start = time.time()
-    low_indices = np.where(point_cloud_world.data[2,:] < min_height)[0] 
-    point_cloud_filtered.data[2,low_indices] = min_height 
+    low_indices = np.where(point_cloud_world.data[2, :] < min_height)[0]
+    point_cloud_filtered.data[2, low_indices] = min_height
 
     # filter high
-    high_indices = np.where(point_cloud_world.data[2,:] > max_height)[0] 
-    point_cloud_filtered.data[2,high_indices] = max_height 
+    high_indices = np.where(point_cloud_world.data[2, :] > max_height)[0]
+    point_cloud_filtered.data[2, high_indices] = max_height
 
     # re-project and update depth im
-    #depth_im_filtered = camera_intr.project_to_image(T_camera_world.inverse() * point_cloud_filtered)
-    logging.info('Clipping took %.3f sec' %(time.time()-clip_start))
-    
+    logging.info("Clipping took %.3f sec" % (time.time() - clip_start))
+
     # vis
-    focal_point = np.mean(point_cloud_filtered.data,
-                          axis=1)
+    focal_point = np.mean(point_cloud_filtered.data, axis=1)
     if vis_clipping:
-        vis3d.figure(camera_pose=T_camera_world.as_frames('camera',
-                                                          'world'),
-                     focal_point=focal_point)
-        vis3d.points(point_cloud_world,
-                     scale=0.001,
-                     color=(1,0,0),
-                     subsample=10)
-        vis3d.points(point_cloud_filtered,
-                     scale=0.001,
-                     color=(0,0,1),
-                     subsample=10)
+        vis3d.figure(
+            camera_pose=T_camera_world.as_frames("camera", "world"),
+            focal_point=focal_point,
+        )
+        vis3d.points(
+            point_cloud_world, scale=0.001, color=(1, 0, 0), subsample=10
+        )
+        vis3d.points(
+            point_cloud_filtered, scale=0.001, color=(0, 0, 1), subsample=10
+        )
         vis3d.show()
-    
+
     pcl_start = time.time()
 
     # subsample point cloud
-    #rate = int(1.0 / rescale_factor)**2
-    #point_cloud_filtered = point_cloud_filtered.subsample(rate, random=False)
-    box = Box(np.array([0.2, -0.24, min_height]), np.array([0.56, 0.21, max_height]), frame='world')
+    # rate = int(1.0 / rescale_factor)**2
+    # point_cloud_filtered = point_cloud_filtered.subsample(rate, random=False)
+    box = Box(
+        np.array([0.2, -0.24, min_height]),
+        np.array([0.56, 0.21, max_height]),
+        frame="world",
+    )
     point_cloud_masked, valid_indices = point_cloud_filtered.box_mask(box)
-    invalid_indices = np.setdiff1d(np.arange(point_cloud_filtered.num_points),
-                                   valid_indices)
-    
+    invalid_indices = np.setdiff1d(
+        np.arange(point_cloud_filtered.num_points), valid_indices
+    )
+
     # apply PCL filters
     pcl_cloud = pcl.PointCloud(point_cloud_masked.data.T.astype(np.float32))
     tree = pcl_cloud.make_kdtree()
     ec = pcl_cloud.make_EuclideanClusterExtraction()
     ec.set_ClusterTolerance(0.005)
-    #ec.set_MinClusterSize(1)
-    #ec.set_MaxClusterSize(250)
+    # ec.set_MinClusterSize(1)
+    # ec.set_MaxClusterSize(250)
     ec.set_MinClusterSize(250)
     ec.set_MaxClusterSize(1000000)
     ec.set_SearchMethod(tree)
@@ -123,84 +127,78 @@
     num_clusters = len(cluster_indices)
 
     segments = []
-    filtered_points = np.zeros([3,point_cloud_masked.num_points])
+    filtered_points = np.zeros([3, point_cloud_masked.num_points])
     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]
 
-        filtered_points[:,cur_i:cur_i+num_points] = points.copy()
+        filtered_points[:, cur_i : cur_i + num_points] = points.copy()
         cur_i = cur_i + num_points
-        
-        seg_point_cloud = PointCloud(points, frame='world')
+
+        seg_point_cloud = PointCloud(points, frame="world")
         segments.append(seg_point_cloud)
 
-    all_points = np.c_[filtered_points[:,:cur_i], point_cloud_filtered.data[:,invalid_indices]]
-    point_cloud_filtered = PointCloud(all_points, frame='world')        
+    all_points = np.c_[
+        filtered_points[:, :cur_i],
+        point_cloud_filtered.data[:, invalid_indices],
+    ]
+    point_cloud_filtered = PointCloud(all_points, frame="world")
     pcl_stop = time.time()
-    logging.info('PCL Seg took %.3f sec' %(pcl_stop-pcl_start))
-        
+    logging.info("PCL Seg took %.3f sec" % (pcl_stop - pcl_start))
+
     if vis_segments:
-        vis3d.figure(camera_pose=T_camera_world.as_frames('camera',
-                                                          'world'),
-                     focal_point=focal_point)
+        vis3d.figure(
+            camera_pose=T_camera_world.as_frames("camera", "world"),
+            focal_point=focal_point,
+        )
         for i, segment in enumerate(segments):
-            color = plt.get_cmap('hsv')(float(i)/num_clusters)[:-1]
-            vis3d.points(segment,
-                         scale=0.001,
-                         color=color,
-                         subsample=5)
+            color = plt.get_cmap("hsv")(float(i) / num_clusters)[:-1]
+            vis3d.points(segment, scale=0.001, color=color, subsample=5)
         vis3d.show()
 
     if vis_final_clouds:
-        vis3d.figure(camera_pose=T_camera_world.as_frames('camera',
-                                                          'world'),
-                     focal_point=focal_point)
-        #vis3d.points(point_cloud_world,
+        vis3d.figure(
+            camera_pose=T_camera_world.as_frames("camera", "world"),
+            focal_point=focal_point,
+        )
+        # vis3d.points(point_cloud_world,
         #             scale=0.001,
         #             color=(1,0,0),
         #             subsample=10)
-        vis3d.points(point_cloud_filtered,
-                     scale=0.001,
-                     color=(0,0,1),
-                     subsample=5)
+        vis3d.points(
+            point_cloud_filtered, scale=0.001, color=(0, 0, 1), subsample=5
+        )
         vis3d.show()
-        
+
     # convert to depth image
     project_start = time.time()
     point_cloud_cam = T_camera_world.inverse() * point_cloud_filtered
-    #depth_im_noise = small_camera_intr.project_to_image(point_cloud_cam)
-    depth_im_filtered = small_camera_intr.project_to_image(point_cloud_cam)    
-    #depth_im_filtered = depth_im_filtered.resize(1.0/rescale_factor)#, interp='nearest')
+    depth_im_filtered = small_camera_intr.project_to_image(point_cloud_cam)
     noise_mask = depth_im_filtered.to_binary()
-    #depth_im_filtered = depth_im_filtered.inpaint(rescale_factor)
-    #depth_im_noise = depth_im_noise.resize(1.0/rescale_factor)
-    #noise_mask = depth_im_noise.to_binary()
-    logging.info('Project took %.3f sec' %(time.time() - project_start))
-    #depth_im_filtered = depth_im.mask_binary(noise_mask)
-    #depth_im_filtered = depth_im_filtered.mask_binary(noise_mask.inverse())
+    logging.info("Project took %.3f sec" % (time.time() - project_start))
     depth_im_filtered = depth_im_filtered.inpaint(0.5)
-    
+
     filter_stop = time.time()
-    logging.info('Filtering took %.3f sec' %(filter_stop-filter_start))
-    
+    logging.info("Filtering took %.3f sec" % (filter_stop - filter_start))
+
     if vis_final_images:
         vis2d.figure()
-        vis2d.subplot(2,2,1)
+        vis2d.subplot(2, 2, 1)
         vis2d.imshow(depth_im)
-        vis2d.title('Orig')
-        vis2d.subplot(2,2,2)
+        vis2d.title("Orig")
+        vis2d.subplot(2, 2, 2)
         vis2d.imshow(depth_im_orig)
-        vis2d.title('Inpainted')
-        vis2d.subplot(2,2,3)
+        vis2d.title("Inpainted")
+        vis2d.subplot(2, 2, 3)
         vis2d.imshow(noise_mask)
-        vis2d.title('Mask')
-        vis2d.subplot(2,2,4)
+        vis2d.title("Mask")
+        vis2d.subplot(2, 2, 4)
         vis2d.imshow(depth_im_filtered)
-        vis2d.title('Filtered')
+        vis2d.title("Filtered")
         vis2d.show()
diff --git a/tools/kinect2_sensor_bridge.py b/tools/kinect2_sensor_bridge.py
new file mode 100755
index 0000000..3fd20c7
--- /dev/null
+++ b/tools/kinect2_sensor_bridge.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+"""
+Interface to the Ensenso N* Sensor
+Author: Jeff Mahler
+"""
+import logging
+import sys
+import time
+import signal
+
+try:
+    import rospy
+except ImportError:
+    logging.warning(
+        "Failed to import ROS in Kinect2_sensor.py. \
+        Kinect will not be able to be used in bridged mode"
+    )
+
+from perception import RgbdSensorFactory
+from perception.kinect2_ros_sensor import Kinect2BridgedQuality
+
+
+def main(args):
+    # from visualization import Visualizer2D as vis2d
+    # from visualization import Visualizer3D as vis3d
+    import matplotlib.pyplot as vis2d
+
+    # set logging
+    logging.getLogger().setLevel(logging.DEBUG)
+    rospy.init_node("kinect_reader", anonymous=True)
+
+    num_frames = 5
+    sensor_cfg = {
+        "quality": Kinect2BridgedQuality.HD,
+        "frame": "kinect2_rgb_optical_frame",
+    }
+    sensor_type = "bridged_kinect2"
+    sensor = RgbdSensorFactory.sensor(sensor_type, sensor_cfg)
+    sensor.start()
+
+    def handler(signum, frame):
+        rospy.loginfo("caught CTRL+C, exiting...")
+        if sensor is not None:
+            sensor.stop()
+        exit(0)
+
+    signal.signal(signal.SIGINT, handler)
+
+    total_time = 0
+    for i in range(num_frames):
+        if i > 0:
+            start_time = time.time()
+
+        _, depth_im, _ = sensor.frames()
+
+        if i > 0:
+            total_time += time.time() - start_time
+            logging.info("Frame %d" % (i))
+            logging.info("Avg FPS: %.5f" % (float(i) / total_time))
+
+    depth_im = sensor.median_depth_img(num_img=5)
+    color_im, depth_im, _ = sensor.frames()
+
+    sensor.stop()
+
+    vis2d.figure()
+    vis2d.subplot("211")
+    vis2d.imshow(depth_im.data)
+    vis2d.title("Kinect - depth Raw")
+
+    vis2d.subplot("212")
+    vis2d.imshow(color_im.data)
+    vis2d.title("kinect color")
+    vis2d.show()
+
+
+if __name__ == "__main__":
+    main(sys.argv)
diff --git a/tools/primesense_viewer.py b/tools/primesense_viewer.py
index d3c96b2..f813c48 100644
--- a/tools/primesense_viewer.py
+++ b/tools/primesense_viewer.py
@@ -11,25 +11,25 @@
 from visualization import Visualizer2D as vis
 from visualization import Visualizer3D as vis3d
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     logging.getLogger().setLevel(logging.INFO)
 
     sensor = PrimesenseSensor()
 
-    logging.info('Starting sensor')
+    logging.info("Starting sensor")
     sensor.start()
     camera_intr = sensor.ir_intrinsics
 
     n = 15
     frame_rates = []
     for i in range(n):
-        logging.info('Reading frame %d of %d' %(i+1, n))
+        logging.info("Reading frame %d of %d" % (i + 1, n))
         read_start = time.time()
         color_im, depth_im, _ = sensor.frames()
         read_stop = time.time()
-        frame_rates.append(1.0/(read_stop-read_start))
+        frame_rates.append(1.0 / (read_stop - read_start))
 
-    logging.info('Avg fps: %.3f' %(np.mean(frame_rates)))
+    logging.info("Avg fps: %.3f" % (np.mean(frame_rates)))
 
     color_im = color_im.inpaint(rescale_factor=0.5)
     depth_im = depth_im.inpaint(rescale_factor=0.5)
@@ -40,11 +40,10 @@
     vis3d.show()
 
     vis.figure()
-    vis.subplot(1,2,1)
+    vis.subplot(1, 2, 1)
     vis.imshow(color_im)
-    vis.subplot(1,2,2)
+    vis.subplot(1, 2, 2)
     vis.imshow(depth_im)
     vis.show()
 
     sensor.stop()
-
diff --git a/tools/register_camera.py b/tools/register_camera.py
index 6c5304b..01b84cd 100644
--- a/tools/register_camera.py
+++ b/tools/register_camera.py
@@ -1,7 +1,7 @@
 """
 Script to register sensors to a chessboard for the YuMi setup
 Authors: Jeff Mahler and Brenton Chu
-""" 
+"""
 import argparse
 import cv2
 import logging
@@ -14,8 +14,15 @@
 import rospy
 import rosgraph.roslogging as rl
 
-from autolab_core import Point, PointCloud, RigidTransform, YamlConfig
-from perception import CameraChessboardRegistration, RgbdSensorFactory
+from autolab_core import (
+    Point,
+    PointCloud,
+    RigidTransform,
+    YamlConfig,
+    keyboard_input,
+    CameraChessboardRegistration,
+)
+from perception import RgbdSensorFactory
 
 from visualization import Visualizer3D as vis3d
 from yumipy import YuMiRobot
@@ -24,100 +31,122 @@
 global clicked_pt
 clicked_pt = None
 pt_radius = 2
-pt_color = (255,0,0)
+pt_color = (255, 0, 0)
+
+
 def click_gripper(event, x, y, flags, param):
     global clicked_pt
     if event == cv2.EVENT_LBUTTONDBLCLK:
-        clicked_pt = np.array([x,y])
-        logging.info('Clicked: {}'.format(clicked_pt))
-        
-if __name__ == '__main__':
+        clicked_pt = np.array([x, y])
+        logging.info("Clicked: {}".format(clicked_pt))
+
+
+if __name__ == "__main__":
     logging.getLogger().setLevel(logging.INFO)
 
     # parse args
-    parser = argparse.ArgumentParser(description='Register a camera to a robot')
-    parser.add_argument('--config_filename', type=str, default='cfg/tools/register_camera.yaml', help='filename of a YAML configuration for registration')
+    parser = argparse.ArgumentParser(
+        description="Register a camera to a robot"
+    )
+    parser.add_argument(
+        "--config_filename",
+        type=str,
+        default="cfg/tools/register_camera.yaml",
+        help="filename of a YAML configuration for registration",
+    )
     args = parser.parse_args()
     config_filename = args.config_filename
     config = YamlConfig(config_filename)
-    
+
     # get known tf from chessboard to world
-    T_cb_world = RigidTransform.load(config['chessboard_tf'])
-    
+    T_cb_world = RigidTransform.load(config["chessboard_tf"])
+
     # initialize node
-    rospy.init_node('register_camera', anonymous=True)
+    rospy.init_node("register_camera", anonymous=True)
     logging.getLogger().addHandler(rl.RosStreamHandler())
 
     # get camera sensor object
-    for sensor_frame, sensor_data in config['sensors'].iteritems():
-        logging.info('Registering %s' %(sensor_frame))
-        sensor_config = sensor_data['sensor_config']
-        registration_config = sensor_data['registration_config'].copy()
-        registration_config.update(config['chessboard_registration'])
-        
+    for sensor_frame, sensor_data in config["sensors"].iteritems():
+        logging.info("Registering %s" % (sensor_frame))
+        sensor_config = sensor_data["sensor_config"]
+        registration_config = sensor_data["registration_config"].copy()
+        registration_config.update(config["chessboard_registration"])
+
         # open sensor
         try:
-            sensor_type = sensor_config['type']
-            sensor_config['frame'] = sensor_frame
-            logging.info('Creating sensor')
+            sensor_type = sensor_config["type"]
+            sensor_config["frame"] = sensor_frame
+            logging.info("Creating sensor")
             sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
-            logging.info('Starting sensor')
+            logging.info("Starting sensor")
             sensor.start()
             ir_intrinsics = sensor.ir_intrinsics
-            logging.info('Sensor initialized')
+            logging.info("Sensor initialized")
 
             # register
-            reg_result = CameraChessboardRegistration.register(sensor, registration_config)
+            reg_result = CameraChessboardRegistration.register(
+                sensor, registration_config
+            )
             T_camera_world = T_cb_world * reg_result.T_camera_cb
 
-            logging.info('Final Result for sensor %s' %(sensor_frame))
-            logging.info('Rotation: ')
+            logging.info("Final Result for sensor %s" % (sensor_frame))
+            logging.info("Rotation: ")
             logging.info(T_camera_world.rotation)
-            logging.info('Quaternion: ')
+            logging.info("Quaternion: ")
             logging.info(T_camera_world.quaternion)
-            logging.info('Translation: ')
+            logging.info("Translation: ")
             logging.info(T_camera_world.translation)
 
-        except Exception as e:
-            logging.error('Failed to register sensor {}'.format(sensor_frame))
+        except Exception:
+            logging.error("Failed to register sensor {}".format(sensor_frame))
             traceback.print_exc()
             continue
 
         # fix the chessboard corners
-        if config['fix_orientation_cb_to_world']:
+        if config["fix_orientation_cb_to_world"]:
             # read params
-            num_pts_x = config['grid_x']
-            num_pts_y = config['grid_y']
-            grid_width = config['grid_width']
-            grid_height = config['grid_height']
-            gripper_height = config['gripper_height']
-            grid_center_x = config['grid_center_x']
-            grid_center_y = config['grid_center_y']
-            
+            num_pts_x = config["grid_x"]
+            num_pts_y = config["grid_y"]
+            grid_width = config["grid_width"]
+            grid_height = config["grid_height"]
+            gripper_height = config["gripper_height"]
+            grid_center_x = config["grid_center_x"]
+            grid_center_y = config["grid_center_y"]
+
             # determine robot poses
             robot_poses = []
             for i in range(num_pts_x):
-                x = -float(grid_width) / 2 + grid_center_x + float(i * grid_width) / num_pts_x
+                x = (
+                    -float(grid_width) / 2
+                    + grid_center_x
+                    + float(i * grid_width) / num_pts_x
+                )
                 for j in range(num_pts_y):
-                    y = -float(grid_height) / 2 + grid_center_y + float(j * grid_height) / num_pts_y
+                    y = (
+                        -float(grid_height) / 2
+                        + grid_center_y
+                        + float(j * grid_height) / num_pts_y
+                    )
 
                     # form robot pose
-                    R_robot_world = np.array([[1, 0, 0],
-                                              [0, 0, 1],
-                                              [0, -1, 0]])
+                    R_robot_world = np.array(
+                        [[1, 0, 0], [0, 0, 1], [0, -1, 0]]
+                    )
                     t_robot_world = np.array([x, y, gripper_height])
-                    T_robot_world = RigidTransform(rotation=R_robot_world,
-                                                   translation=t_robot_world,
-                                                   from_frame='gripper',
-                                                   to_frame='world')
+                    T_robot_world = RigidTransform(
+                        rotation=R_robot_world,
+                        translation=t_robot_world,
+                        from_frame="gripper",
+                        to_frame="world",
+                    )
                     robot_poses.append(T_robot_world)
 
             # start robot
             y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
-            y.set_z('fine')
+            y.set_z("fine")
             y.reset_home()
             global clicked_pt
-            
+
             # iteratively go to poses
             robot_points_camera = []
             for robot_pose in robot_poses:
@@ -126,47 +155,57 @@ def click_gripper(event, x, y, flags, param):
 
                 # move to pose
                 y.right.goto_pose(robot_pose, wait_for_res=True)
-                    
+
                 # capture image
                 color_im, depth_im, _ = sensor.frames()
                 depth_im = depth_im.inpaint(0.25)
-                cv2.namedWindow('click')
-                cv2.setMouseCallback('click', click_gripper)
+                cv2.namedWindow("click")
+                cv2.setMouseCallback("click", click_gripper)
                 while True:
                     if clicked_pt is None:
-                        cv2.imshow('click', color_im.data)
+                        cv2.imshow("click", color_im.data)
                     else:
                         im = color_im.data.copy()
-                        cv2.circle(im,
-                                   tuple(clicked_pt.tolist()),
-                                   pt_radius,
-                                   pt_color,
-                                   -1)
-                        cv2.imshow('click', im)
+                        cv2.circle(
+                            im,
+                            tuple(clicked_pt.tolist()),
+                            pt_radius,
+                            pt_color,
+                            -1,
+                        )
+                        cv2.imshow("click", im)
                     key = cv2.waitKey(1) & 0xFF
-                    if key == ord('q') and clicked_pt is not None:
-                        logging.info('Moving to next pose')
+                    if key == ord("q") and clicked_pt is not None:
+                        logging.info("Moving to next pose")
                         break
 
                 # store clicked pt in 3D
-                logging.info('Point collection complete')
+                logging.info("Point collection complete")
                 depth = depth_im[clicked_pt[1], clicked_pt[0]]
-                p = ir_intrinsics.deproject_pixel(depth,
-                                                  Point(clicked_pt, frame=ir_intrinsics.frame))
+                p = ir_intrinsics.deproject_pixel(
+                    depth, Point(clicked_pt, frame=ir_intrinsics.frame)
+                )
 
                 robot_points_camera.append(p.data)
 
             # reset
             y.reset_home()
             y.stop()
-            
+
             # collect
-            true_robot_points_world = PointCloud(np.array([T.translation for T in robot_poses]).T,
-                                                 frame=ir_intrinsics.frame)
-            est_robot_points_world = T_camera_world * PointCloud(np.array(robot_points_camera).T,
-                                                                 frame=ir_intrinsics.frame)
-            mean_true_robot_point = np.mean(true_robot_points_world.data, axis=1).reshape(3,1)
-            mean_est_robot_point = np.mean(est_robot_points_world.data, axis=1).reshape(3,1)
+            true_robot_points_world = PointCloud(
+                np.array([T.translation for T in robot_poses]).T,
+                frame=ir_intrinsics.frame,
+            )
+            est_robot_points_world = T_camera_world * PointCloud(
+                np.array(robot_points_camera).T, frame=ir_intrinsics.frame
+            )
+            mean_true_robot_point = np.mean(
+                true_robot_points_world.data, axis=1
+            ).reshape(3, 1)
+            mean_est_robot_point = np.mean(
+                est_robot_points_world.data, axis=1
+            ).reshape(3, 1)
 
             # fit a plane
             best_R_cb_world = None
@@ -177,30 +216,46 @@ def click_gripper(event, x, y, flags, param):
             sample_size = int(num_poses * 0.3)
             min_inliers = int(num_poses * 0.6)
             dist_thresh = 0.0015
-            true_robot_points_world._data = true_robot_points_world._data - mean_true_robot_point
-            est_robot_points_world._data = est_robot_points_world._data - mean_est_robot_point
+            true_robot_points_world._data = (
+                true_robot_points_world._data - mean_true_robot_point
+            )
+            est_robot_points_world._data = (
+                est_robot_points_world._data - mean_est_robot_point
+            )
             while k < K:
-                ind = np.random.choice(num_poses, size=sample_size, replace=False)
-                H = est_robot_points_world.data[:,ind].dot(true_robot_points_world.data[:,ind].T)
+                ind = np.random.choice(
+                    num_poses, size=sample_size, replace=False
+                )
+                H = est_robot_points_world.data[:, ind].dot(
+                    true_robot_points_world.data[:, ind].T
+                )
                 U, S, V = np.linalg.svd(H)
                 R_cb_world = V.T.dot(U.T)
-                
-                fixed_robot_points_world = R_cb_world.dot(est_robot_points_world.data)
+
+                fixed_robot_points_world = R_cb_world.dot(
+                    est_robot_points_world.data
+                )
                 diffs = fixed_robot_points_world - true_robot_points_world.data
                 dists = np.linalg.norm(diffs, axis=0)
                 inliers = np.where(dists < dist_thresh)[0]
                 num_inliers = inliers.shape[0]
 
-                print k, num_inliers, np.mean(dists)
+                print(k, num_inliers, np.mean(dists))
 
                 if num_inliers >= min_inliers:
                     ind = inliers
-                    H = est_robot_points_world.data[:,ind].dot(true_robot_points_world.data[:,ind].T)
+                    H = est_robot_points_world.data[:, ind].dot(
+                        true_robot_points_world.data[:, ind].T
+                    )
                     U, S, V = np.linalg.svd(H)
                     R_cb_world = V.T.dot(U.T)
-                
-                    fixed_robot_points_world = R_cb_world.dot(est_robot_points_world.data)
-                    diffs = fixed_robot_points_world - true_robot_points_world.data
+
+                    fixed_robot_points_world = R_cb_world.dot(
+                        est_robot_points_world.data
+                    )
+                    diffs = (
+                        fixed_robot_points_world - true_robot_points_world.data
+                    )
                     dists = np.linalg.norm(diffs, axis=0)
 
                     mean_dist = np.mean(dists[ind])
@@ -208,58 +263,95 @@ def click_gripper(event, x, y, flags, param):
                         best_dist = mean_dist
                         best_R_cb_world = R_cb_world
                 k += 1
-                        
+
             R_cb_world = best_R_cb_world
-            T_corrected_cb_world = RigidTransform(rotation=R_cb_world,
-                                                  from_frame='world',
-                                                  to_frame='world')
+            T_corrected_cb_world = RigidTransform(
+                rotation=R_cb_world, from_frame="world", to_frame="world"
+            )
             R_cb_world = R_cb_world.dot(T_cb_world.rotation)
-            T_cb_world = RigidTransform(rotation=R_cb_world,
-                                        translation=T_cb_world.translation,
-                                        from_frame=T_cb_world.from_frame,
-                                        to_frame=T_cb_world.to_frame)
+            T_cb_world = RigidTransform(
+                rotation=R_cb_world,
+                translation=T_cb_world.translation,
+                from_frame=T_cb_world.from_frame,
+                to_frame=T_cb_world.to_frame,
+            )
             T_camera_world = T_cb_world * reg_result.T_camera_cb
-            T_cb_world.save(config['chessboard_tf'])
-            
+            T_cb_world.save(config["chessboard_tf"])
+
             # vis
-            if config['vis_points']:
+            if config["vis_points"]:
                 _, depth_im, _ = sensor.frames()
-                points_world = T_camera_world * ir_intrinsics.deproject(depth_im)
-                true_robot_points_world = PointCloud(np.array([T.translation for T in robot_poses]).T,
-                                                     frame=ir_intrinsics.frame)
-                est_robot_points_world = T_camera_world * PointCloud(np.array(robot_points_camera).T,
-                                                                     frame=ir_intrinsics.frame)
-                mean_est_robot_point = np.mean(est_robot_points_world.data, axis=1).reshape(3,1)
-                est_robot_points_world._data = est_robot_points_world._data - mean_est_robot_point + mean_true_robot_point
-                fixed_robot_points_world = T_corrected_cb_world * est_robot_points_world
-                mean_fixed_robot_point = np.mean(fixed_robot_points_world.data, axis=1).reshape(3,1)
-                fixed_robot_points_world._data = fixed_robot_points_world._data - mean_fixed_robot_point + mean_true_robot_point
+                points_world = T_camera_world * ir_intrinsics.deproject(
+                    depth_im
+                )
+                true_robot_points_world = PointCloud(
+                    np.array([T.translation for T in robot_poses]).T,
+                    frame=ir_intrinsics.frame,
+                )
+                est_robot_points_world = T_camera_world * PointCloud(
+                    np.array(robot_points_camera).T, frame=ir_intrinsics.frame
+                )
+                mean_est_robot_point = np.mean(
+                    est_robot_points_world.data, axis=1
+                ).reshape(3, 1)
+                est_robot_points_world._data = (
+                    est_robot_points_world._data
+                    - mean_est_robot_point
+                    + mean_true_robot_point
+                )
+                fixed_robot_points_world = (
+                    T_corrected_cb_world * est_robot_points_world
+                )
+                mean_fixed_robot_point = np.mean(
+                    fixed_robot_points_world.data, axis=1
+                ).reshape(3, 1)
+                fixed_robot_points_world._data = (
+                    fixed_robot_points_world._data
+                    - mean_fixed_robot_point
+                    + mean_true_robot_point
+                )
                 vis3d.figure()
-                vis3d.points(points_world, color=(0,1,0), subsample=10, random=True, scale=0.001)
-                vis3d.points(true_robot_points_world, color=(0,0,1), scale=0.001)
-                vis3d.points(fixed_robot_points_world, color=(1,1,0), scale=0.001)
-                vis3d.points(est_robot_points_world, color=(1,0,0), scale=0.001)
+                vis3d.points(
+                    points_world,
+                    color=(0, 1, 0),
+                    subsample=10,
+                    random=True,
+                    scale=0.001,
+                )
+                vis3d.points(
+                    true_robot_points_world, color=(0, 0, 1), scale=0.001
+                )
+                vis3d.points(
+                    fixed_robot_points_world, color=(1, 1, 0), scale=0.001
+                )
+                vis3d.points(
+                    est_robot_points_world, color=(1, 0, 0), scale=0.001
+                )
                 vis3d.pose(T_camera_world)
                 vis3d.show()
 
         # save tranformation arrays based on setup
-        output_dir = os.path.join(config['calib_dir'], sensor_frame)
+        output_dir = os.path.join(config["calib_dir"], sensor_frame)
         if not os.path.exists(output_dir):
             os.makedirs(output_dir)
-        pose_filename = os.path.join(output_dir, '%s_to_world.tf' %(sensor_frame))
+        pose_filename = os.path.join(
+            output_dir, "%s_to_world.tf" % (sensor_frame)
+        )
         T_camera_world.save(pose_filename)
-        intr_filename = os.path.join(output_dir, '%s.intr' %(sensor_frame))
+        intr_filename = os.path.join(output_dir, "%s.intr" % (sensor_frame))
         ir_intrinsics.save(intr_filename)
-        f = open(os.path.join(output_dir, 'corners_cb_%s.npy' %(sensor_frame)), 'w')
+        f = open(
+            os.path.join(output_dir, "corners_cb_%s.npy" % (sensor_frame)), "w"
+        )
         np.save(f, reg_result.cb_points_cam.data)
-                
+
         # move the robot to the chessboard center for verification
-        if config['use_robot']:  
+        if config["use_robot"]:
             # get robot type
-            robot_type = 'yumi'
-            if 'robot_type' in config.keys():
-                robot_type = config['robot_type']
-            
+            robot_type = "yumi"
+            if "robot_type" in config.keys():
+                robot_type = config["robot_type"]
+
             # find the rightmost and further cb point in world frame
             cb_points_world = T_camera_world * reg_result.cb_points_cam
             cb_point_data_world = cb_points_world.data
@@ -268,8 +360,12 @@ def click_gripper(event, x, y, flags, param):
             ip = dir_world.dot(cb_point_data_world)
 
             # open interface to robot
-            if robot_type == 'ur5':
-                from ur_control import UniversalRobot, ToolState, T_KINEMATIC_AVOIDANCE_WORLD, KINEMATIC_AVOIDANCE_JOINTS
+            if robot_type == "ur5":
+                from ur_control import (
+                    UniversalRobot,
+                    KINEMATIC_AVOIDANCE_JOINTS,
+                )
+
                 robot = UniversalRobot()
                 robot.reset_home()
             else:
@@ -278,39 +374,58 @@ def click_gripper(event, x, y, flags, param):
                 robot = y.right
                 waypoints = []
             time.sleep(1)
-                
+
             # choose target point #1
             target_ind = np.where(ip == np.max(ip))[0]
             target_pt_world = cb_points_world[target_ind[0]]
-                
+
             # create robot pose relative to target point
-            if robot_type == 'ur5':
-                R_gripper_world = np.array([[-1.0, 0, 0],
-                                            [0, 1.0, 0],
-                                            [0, 0, -1.0]])
+            if robot_type == "ur5":
+                R_gripper_world = np.array(
+                    [[-1.0, 0, 0], [0, 1.0, 0], [0, 0, -1.0]]
+                )
             else:
-                R_gripper_world = np.array([[1.0, 0, 0],
-                                            [0, -1.0, 0],
-                                            [0, 0, -1.0]])
-            t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'],
-                                        target_pt_world.y + config['gripper_offset_y'],
-                                        target_pt_world.z + config['gripper_offset_z']])
-            T_gripper_world = RigidTransform(rotation=R_gripper_world,
-                                             translation=t_gripper_world,
-                                             from_frame='gripper',
-                                             to_frame='cb')
-            logging.info('Moving robot to point x=%f, y=%f, z=%f' %(t_gripper_world[0], t_gripper_world[1], t_gripper_world[2]))
-
-            T_lift = RigidTransform(translation=(0,0,0.05), from_frame='cb', to_frame='cb')
+                R_gripper_world = np.array(
+                    [[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]]
+                )
+            t_gripper_world = np.array(
+                [
+                    target_pt_world.x + config["gripper_offset_x"],
+                    target_pt_world.y + config["gripper_offset_y"],
+                    target_pt_world.z + config["gripper_offset_z"],
+                ]
+            )
+            T_gripper_world = RigidTransform(
+                rotation=R_gripper_world,
+                translation=t_gripper_world,
+                from_frame="gripper",
+                to_frame="cb",
+            )
+            logging.info(
+                "Moving robot to point x=%f, y=%f, z=%f"
+                % (t_gripper_world[0], t_gripper_world[1], t_gripper_world[2])
+            )
+
+            T_lift = RigidTransform(
+                translation=(0, 0, 0.05), from_frame="cb", to_frame="cb"
+            )
             T_gripper_world_lift = T_lift * T_gripper_world
             T_orig_gripper_world_lift = T_gripper_world_lift.copy()
 
-            if config['vis_cb_corners']:
+            if config["vis_cb_corners"]:
                 _, depth_im, _ = sensor.frames()
-                points_world = T_camera_world * ir_intrinsics.deproject(depth_im)
+                points_world = T_camera_world * ir_intrinsics.deproject(
+                    depth_im
+                )
                 vis3d.figure()
-                vis3d.points(cb_points_world, color=(0,0,1), scale=0.005)
-                vis3d.points(points_world, color=(0,1,0), subsample=10, random=True, scale=0.001)
+                vis3d.points(cb_points_world, color=(0, 0, 1), scale=0.005)
+                vis3d.points(
+                    points_world,
+                    color=(0, 1, 0),
+                    subsample=10,
+                    random=True,
+                    scale=0.001,
+                )
                 vis3d.pose(T_camera_world)
                 vis3d.pose(T_gripper_world_lift)
                 vis3d.pose(T_gripper_world)
@@ -319,38 +434,49 @@ def click_gripper(event, x, y, flags, param):
                 vis3d.table(dim=0.5, T_table_world=T_cb_world)
                 vis3d.show()
 
-            if robot_type == 'ur5':
+            if robot_type == "ur5":
                 robot.movej(KINEMATIC_AVOIDANCE_JOINTS, wait_for_res=True)
                 robot.goto_pose(T_gripper_world_lift)
             else:
                 robot.goto_pose(T_gripper_world_lift)
             robot.goto_pose(T_gripper_world)
-            
+
             # wait for human measurement
-            yesno = raw_input('Take measurement. Hit [ENTER] when done')
+            keyboard_input("Take measurement. Hit [ENTER] when done")
             robot.goto_pose(T_gripper_world_lift)
 
             # choose target point 2
             target_ind = np.where(ip == np.min(ip))[0]
             target_pt_world = cb_points_world[target_ind[0]]
-                
+
             # create robot pose relative to target point
-            t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'],
-                                        target_pt_world.y + config['gripper_offset_y'],
-                                        target_pt_world.z + config['gripper_offset_z']])
-            T_gripper_world = RigidTransform(rotation=R_gripper_world,
-                                             translation=t_gripper_world,
-                                             from_frame='gripper',
-                                             to_frame='cb')
-            logging.info('Moving robot to point x=%f, y=%f, z=%f' %(t_gripper_world[0], t_gripper_world[1], t_gripper_world[2]))
-            
-            T_lift = RigidTransform(translation=(0,0,0.05), from_frame='cb', to_frame='cb')
+            t_gripper_world = np.array(
+                [
+                    target_pt_world.x + config["gripper_offset_x"],
+                    target_pt_world.y + config["gripper_offset_y"],
+                    target_pt_world.z + config["gripper_offset_z"],
+                ]
+            )
+            T_gripper_world = RigidTransform(
+                rotation=R_gripper_world,
+                translation=t_gripper_world,
+                from_frame="gripper",
+                to_frame="cb",
+            )
+            logging.info(
+                "Moving robot to point x=%f, y=%f, z=%f"
+                % (t_gripper_world[0], t_gripper_world[1], t_gripper_world[2])
+            )
+
+            T_lift = RigidTransform(
+                translation=(0, 0, 0.05), from_frame="cb", to_frame="cb"
+            )
             T_gripper_world_lift = T_lift * T_gripper_world
             robot.goto_pose(T_gripper_world_lift)
             robot.goto_pose(T_gripper_world)
-            
+
             # wait for human measurement
-            yesno = raw_input('Take measurement. Hit [ENTER] when done')
+            keyboard_input("Take measurement. Hit [ENTER] when done")
             robot.goto_pose(T_gripper_world_lift)
             robot.goto_pose(T_orig_gripper_world_lift)
 
@@ -360,35 +486,49 @@ def click_gripper(event, x, y, flags, param):
             ip = dir_world.dot(cb_point_data_world)
             target_ind = np.where(ip == np.max(ip))[0]
             target_pt_world = cb_points_world[target_ind[0]]
-                
+
             # create robot pose relative to target point
-            t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'],
-                                        target_pt_world.y + config['gripper_offset_y'],
-                                        target_pt_world.z + config['gripper_offset_z']])
-            T_gripper_world = RigidTransform(rotation=R_gripper_world,
-                                             translation=t_gripper_world,
-                                             from_frame='gripper',
-                                             to_frame='cb')
-            logging.info('Moving robot to point x=%f, y=%f, z=%f' %(t_gripper_world[0], t_gripper_world[1], t_gripper_world[2]))
-            
-            T_lift = RigidTransform(translation=(0,0,0.05), from_frame='cb', to_frame='cb')
+            t_gripper_world = np.array(
+                [
+                    target_pt_world.x + config["gripper_offset_x"],
+                    target_pt_world.y + config["gripper_offset_y"],
+                    target_pt_world.z + config["gripper_offset_z"],
+                ]
+            )
+            T_gripper_world = RigidTransform(
+                rotation=R_gripper_world,
+                translation=t_gripper_world,
+                from_frame="gripper",
+                to_frame="cb",
+            )
+            logging.info(
+                "Moving robot to point x=%f, y=%f, z=%f"
+                % (t_gripper_world[0], t_gripper_world[1], t_gripper_world[2])
+            )
+
+            T_lift = RigidTransform(
+                translation=(0, 0, 0.05), from_frame="cb", to_frame="cb"
+            )
             T_gripper_world_lift = T_lift * T_gripper_world
             robot.goto_pose(T_gripper_world_lift)
             robot.goto_pose(T_gripper_world)
-            
+
             # wait for human measurement
-            yesno = raw_input('Take measurement. Hit [ENTER] when done')
+            keyboard_input("Take measurement. Hit [ENTER] when done")
             robot.goto_pose(T_gripper_world_lift)
             robot.goto_pose(T_orig_gripper_world_lift)
-            
+
             # stop robot
             robot.reset_home()
-            if robot_type != 'ur5' and 'reset_bin' in config.keys() and config['reset_bin']:
+            if (
+                robot_type != "ur5"
+                and "reset_bin" in config.keys()
+                and config["reset_bin"]
+            ):
                 y.reset_bin()
-            if robot_type == 'ur5':
+            if robot_type == "ur5":
                 robot.stop()
             else:
                 y.stop()
-                
+
         sensor.stop()
-            
diff --git a/tools/register_ensenso.py b/tools/register_ensenso.py
index 01c5885..6565bc0 100644
--- a/tools/register_ensenso.py
+++ b/tools/register_ensenso.py
@@ -11,11 +11,12 @@
 from ensenso.srv import CollectPattern, EstimatePatternPose
 import rospy
 
-from autolab_core import Point, RigidTransform, YamlConfig
-from perception import EnsensoSensor
+from autolab_core import Point, RigidTransform, YamlConfig, keyboard_input
+from perception import RgbdSensorFactory
 from yumipy import YuMiRobot
 from yumipy import YuMiConstants as YMC
 
+
 def register_ensenso(config):
     # set parameters
     average = True
@@ -25,127 +26,152 @@ def register_ensenso(config):
     grid_spacing = -1
 
     # read parameters
-    num_patterns = config['num_patterns']
-    max_tries = config['max_tries']
-    
+    num_patterns = config["num_patterns"]
+    max_tries = config["max_tries"]
+
     # load tf pattern to world
-    T_pattern_world = RigidTransform.load(config['pattern_tf'])
-    
+    T_pattern_world = RigidTransform.load(config["pattern_tf"])
+
     # initialize sensor
-    sensor_frame = config['sensor_frame']
-    sensor = EnsensoSensor(sensor_frame)
+    sensor_frame = config["sensor_frame"]
+    sensor_config = {"frame": sensor_frame}
+    logging.info("Creating sensor")
+    sensor = RgbdSensorFactory.sensor("ensenso", sensor_config)
 
     # initialize node
-    rospy.init_node('register_ensenso', anonymous=True)
-    rospy.wait_for_service('/%s/collect_pattern' %(sensor_frame))
-    rospy.wait_for_service('/%s/estimate_pattern_pose' %(sensor_frame))
-    
+    rospy.init_node("register_ensenso", anonymous=True)
+    rospy.wait_for_service("/%s/collect_pattern" % (sensor_frame))
+    rospy.wait_for_service("/%s/estimate_pattern_pose" % (sensor_frame))
+
     # start sensor
-    print('Starting sensor')
+    print("Starting sensor")
     sensor.start()
     time.sleep(1)
-    print('Sensor initialized')
-    
+    print("Sensor initialized")
+
     # perform registration
     try:
-        print('Collecting patterns')
+        print("Collecting patterns")
         num_detected = 0
         i = 0
         while num_detected < num_patterns and i < max_tries:
-            collect_pattern = rospy.ServiceProxy('/%s/collect_pattern' %(sensor_frame), CollectPattern)
-            resp = collect_pattern(add_to_buffer,
-                                   clear_buffer,
-                                   decode,
-                                   grid_spacing)
+            collect_pattern = rospy.ServiceProxy(
+                "/%s/collect_pattern" % (sensor_frame), CollectPattern
+            )
+            resp = collect_pattern(
+                add_to_buffer, clear_buffer, decode, grid_spacing
+            )
             if resp.success:
-                print('Detected pattern %d' %(num_detected))
+                print("Detected pattern %d" % (num_detected))
                 num_detected += 1
             i += 1
 
         if i == max_tries:
-            raise ValueError('Failed to detect calibration pattern!')
-            
-        print('Estimating pattern pose')
-        estimate_pattern_pose = rospy.ServiceProxy('/%s/estimate_pattern_pose' %(sensor_frame), EstimatePatternPose)
+            raise ValueError("Failed to detect calibration pattern!")
+
+        print("Estimating pattern pose")
+        estimate_pattern_pose = rospy.ServiceProxy(
+            "/%s/estimate_pattern_pose" % (sensor_frame), EstimatePatternPose
+        )
         resp = estimate_pattern_pose(average)
 
-        q_pattern_camera = np.array([resp.pose.orientation.w,
-                                     resp.pose.orientation.x,
-                                     resp.pose.orientation.y,
-                                     resp.pose.orientation.z])
-        t_pattern_camera = np.array([resp.pose.position.x,
-                                     resp.pose.position.y,
-                                     resp.pose.position.z])
-        T_pattern_camera = RigidTransform(rotation=q_pattern_camera,
-                                          translation=t_pattern_camera,
-                                          from_frame='pattern',
-                                          to_frame=sensor_frame)
+        q_pattern_camera = np.array(
+            [
+                resp.pose.orientation.w,
+                resp.pose.orientation.x,
+                resp.pose.orientation.y,
+                resp.pose.orientation.z,
+            ]
+        )
+        t_pattern_camera = np.array(
+            [resp.pose.position.x, resp.pose.position.y, resp.pose.position.z]
+        )
+        T_pattern_camera = RigidTransform(
+            rotation=q_pattern_camera,
+            translation=t_pattern_camera,
+            from_frame="pattern",
+            to_frame=sensor_frame,
+        )
     except rospy.ServiceException as e:
-        print('Service call failed: %s' %(str(e)))
+        print("Service call failed: %s" % (str(e)))
 
     # compute final transformation
     T_camera_world = T_pattern_world * T_pattern_camera.inverse()
-    
+
     # save final transformation
-    output_dir = os.path.join(config['calib_dir'], sensor_frame)
+    output_dir = os.path.join(config["calib_dir"], sensor_frame)
     if not os.path.exists(output_dir):
         os.makedirs(output_dir)
-    pose_filename = os.path.join(output_dir, '%s_to_world.tf' %(sensor_frame))
+    pose_filename = os.path.join(output_dir, "%s_to_world.tf" % (sensor_frame))
     T_camera_world.save(pose_filename)
-    intr_filename = os.path.join(output_dir, '%s.intr' %(sensor_frame))
+    intr_filename = os.path.join(output_dir, "%s.intr" % (sensor_frame))
     sensor.ir_intrinsics.save(intr_filename)
 
     # log final transformation
-    print('Final Result for sensor %s' %(sensor_frame))
-    print('Rotation: ')
+    print("Final Result for sensor %s" % (sensor_frame))
+    print("Rotation: ")
     print(T_camera_world.rotation)
-    print('Quaternion: ')
+    print("Quaternion: ")
     print(T_camera_world.quaternion)
-    print('Translation: ')
+    print("Translation: ")
     print(T_camera_world.translation)
 
     # stop sensor
     sensor.stop()
 
     # move robot to calib pattern center
-    if config['use_robot']:  
+    if config["use_robot"]:
         # create robot pose relative to target point
         target_pt_camera = Point(T_pattern_camera.translation, sensor_frame)
         target_pt_world = T_camera_world * target_pt_camera
-        R_gripper_world = np.array([[1.0, 0, 0],
-                                    [0, -1.0, 0],
-                                    [0, 0, -1.0]])
-        t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'],
-                                    target_pt_world.y + config['gripper_offset_y'],
-                                    target_pt_world.z + config['gripper_offset_z']])
-        T_gripper_world = RigidTransform(rotation=R_gripper_world,
-                                         translation=t_gripper_world,
-                                         from_frame='gripper',
-                                         to_frame='world')
+        R_gripper_world = np.array([[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]])
+        t_gripper_world = np.array(
+            [
+                target_pt_world.x + config["gripper_offset_x"],
+                target_pt_world.y + config["gripper_offset_y"],
+                target_pt_world.z + config["gripper_offset_z"],
+            ]
+        )
+        T_gripper_world = RigidTransform(
+            rotation=R_gripper_world,
+            translation=t_gripper_world,
+            from_frame="gripper",
+            to_frame="world",
+        )
 
         # move robot to pose
         y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
         y.reset_home()
         time.sleep(1)
 
-        T_lift = RigidTransform(translation=(0,0,0.05), from_frame='world', to_frame='world')
+        T_lift = RigidTransform(
+            translation=(0, 0, 0.05), from_frame="world", to_frame="world"
+        )
         T_gripper_world_lift = T_lift * T_gripper_world
 
         y.right.goto_pose(T_gripper_world_lift)
         y.right.goto_pose(T_gripper_world)
-        
+
         # wait for human measurement
-        yesno = raw_input('Take measurement. Hit [ENTER] when done')
+        keyboard_input("Take measurement. Hit [ENTER] when done")
         y.right.goto_pose(T_gripper_world_lift)
         y.reset_home()
         y.stop()
-    
-if __name__ == '__main__':
+
+
+if __name__ == "__main__":
     logging.getLogger().setLevel(logging.INFO)
 
     # parse args
-    parser = argparse.ArgumentParser(description='Register a camera to a robot')
-    parser.add_argument('--config_filename', type=str, default='cfg/tools/register_ensenso.yaml', help='filename of a YAML configuration for registration')
+    parser = argparse.ArgumentParser(
+        description="Register a camera to a robot"
+    )
+    parser.add_argument(
+        "--config_filename",
+        type=str,
+        default="cfg/tools/register_ensenso.yaml",
+        help="filename of a YAML configuration for registration",
+    )
     args = parser.parse_args()
     config_filename = args.config_filename
     config = YamlConfig(config_filename)
diff --git a/tools/register_object.py b/tools/register_object.py
index a1a8fdd..0e09820 100644
--- a/tools/register_object.py
+++ b/tools/register_object.py
@@ -1,84 +1,112 @@
-'''
-Script to register an object in world frame provided transform from cam to cb and from obj to cb
+"""
+Script to register an object in world frame provided transform
+from cam to cb and from obj to cb
 Author: Jacky
-'''
+"""
 import os
 import argparse
-import logging 
+import logging
+import trimesh
 
-from autolab_core import RigidTransform, YamlConfig
-from perception import CameraChessboardRegistration, RgbdSensorFactory
+from autolab_core import (
+    RigidTransform,
+    YamlConfig,
+    CameraChessboardRegistration,
+)
+from perception import RgbdSensorFactory
 
 VIS_SUPPORTED = True
 try:
-    from meshpy import ObjFile
     from visualization import Visualizer3D as vis
-except:
-    print 'Failed to import AUTOLAB meshpy and / or visualization package. Visualizatoin disabled'
-    VIS_SUPPORTED = FALSE
+except ImportError:
+    logging.warning(
+        "Failed to import visualization package. Visualization disabled"
+    )
+    VIS_SUPPORTED = False
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     logging.getLogger().setLevel(logging.INFO)
-    
+
     parser = argparse.ArgumentParser()
-    parser.add_argument('object_name')
+    parser.add_argument("object_name")
     args = parser.parse_args()
 
-    config_filename = 'cfg/tools/register_object.yaml'
+    config_filename = "cfg/tools/register_object.yaml"
     config = YamlConfig(config_filename)
-    
-    sensor_frame = config['sensor']['frame_name']
-    sensor_type = config['sensor']['type']
-    sensor_config = config['sensor']
 
-    object_path = os.path.join(config['objects_dir'], args.object_name)
-    obj_cb_transform_file_path = os.path.join(object_path, 'T_cb_{0}.tf'.format(args.object_name))
+    sensor_frame = config["sensor"]["frame_name"]
+    sensor_type = config["sensor"]["type"]
+    sensor_config = config["sensor"]
+
+    object_path = os.path.join(config["objects_dir"], args.object_name)
+    obj_cb_transform_file_path = os.path.join(
+        object_path, "T_cb_{0}.tf".format(args.object_name)
+    )
 
     # load T_cb_obj
     T_cb_obj = RigidTransform.load(obj_cb_transform_file_path)
-    
-    # load T_world_cam 
-    T_world_cam_path = os.path.join(config['calib_dir'], sensor_frame, '{0}_to_world.tf'.format(sensor_frame))
+
+    # load T_world_cam
+    T_world_cam_path = os.path.join(
+        config["calib_dir"],
+        sensor_frame,
+        "{0}_to_world.tf".format(sensor_frame),
+    )
     T_world_cam = RigidTransform.load(T_world_cam_path)
 
     # open sensor
-    sensor_type = sensor_config['type']
-    sensor_config['frame'] = sensor_frame
+    sensor_type = sensor_config["type"]
+    sensor_config["frame"] = sensor_frame
     sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
-    logging.info('Starting sensor')
+    logging.info("Starting sensor")
     sensor.start()
     ir_intrinsics = sensor.ir_intrinsics
-    logging.info('Sensor initialized')
+    logging.info("Sensor initialized")
 
     # register
-    reg_result = CameraChessboardRegistration.register(sensor, config['chessboard_registration'])
-    
+    reg_result = CameraChessboardRegistration.register(
+        sensor, config["chessboard_registration"]
+    )
+
     T_cb_cam = reg_result.T_camera_cb
     T_world_obj = T_world_cam * T_cb_cam.inverse() * T_cb_obj
 
-    output_path = os.path.join(config['calib_dir'], T_world_obj.from_frame)
+    output_path = os.path.join(config["calib_dir"], T_world_obj.from_frame)
     if not os.path.exists(output_path):
         os.makedirs(output_path)
-        
-    output_filename = os.path.join(output_path, '{0}_to_world.tf'.format(T_world_obj.from_frame))
-    print T_world_obj
+
+    output_filename = os.path.join(
+        output_path, "{0}_to_world.tf".format(T_world_obj.from_frame)
+    )
+    logging.info(T_world_obj)
     T_world_obj.save(output_filename)
 
-    if config['vis'] and VIS_SUPPORTED:
+    if config["vis"] and VIS_SUPPORTED:
 
         _, depth_im, _ = sensor.frames()
         pc_cam = ir_intrinsics.deproject(depth_im)
         pc_world = T_world_cam * pc_cam
 
-        mesh_file = ObjFile(os.path.join(object_path, '{0}.obj'.format(args.object_name)))
-        mesh = mesh_file.read()
+        mesh = trimesh.load(
+            os.path.join(object_path, "{0}.obj".format(args.object_name))
+        )
 
-        vis.figure(bgcolor=(0.7,0.7,0.7))
-        vis.mesh(mesh, T_world_obj.as_frames('obj', 'world'), style='surface')
+        vis.figure(bgcolor=(0.7, 0.7, 0.7))
+        vis.mesh(mesh, T_world_obj.as_frames("obj", "world"), style="surface")
         vis.pose(T_world_obj, alpha=0.04, tube_radius=0.002, center_scale=0.01)
-        vis.pose(RigidTransform(from_frame='origin'), alpha=0.04, tube_radius=0.002, center_scale=0.01)
+        vis.pose(
+            RigidTransform(from_frame="origin"),
+            alpha=0.04,
+            tube_radius=0.002,
+            center_scale=0.01,
+        )
         vis.pose(T_world_cam, alpha=0.04, tube_radius=0.002, center_scale=0.01)
-        vis.pose(T_world_cam  * T_cb_cam.inverse(), alpha=0.04, tube_radius=0.002, center_scale=0.01)
+        vis.pose(
+            T_world_cam * T_cb_cam.inverse(),
+            alpha=0.04,
+            tube_radius=0.002,
+            center_scale=0.01,
+        )
         vis.points(pc_world, subsample=20)
         vis.show()
     sensor.stop()
diff --git a/tools/register_webcam.py b/tools/register_webcam.py
index 9096303..d3df860 100644
--- a/tools/register_webcam.py
+++ b/tools/register_webcam.py
@@ -1,7 +1,7 @@
 """
 Script to register webcam to a chessboard in the YuMi setup.
 Authors: Matt Matl and Jeff Mahler
-""" 
+"""
 import argparse
 import cv2
 import logging
@@ -12,86 +12,101 @@
 from autolab_core import RigidTransform, YamlConfig
 from perception import RgbdSensorFactory
 
-from visualization import Visualizer2D as vis2d
-from visualization import Visualizer3D as vis3d
-
-if __name__ == '__main__':
+if __name__ == "__main__":
     logging.getLogger().setLevel(logging.INFO)
 
     # parse args
-    parser = argparse.ArgumentParser(description='Register a webcam to the robot')
-    parser.add_argument('--config_filename', type=str, default='cfg/tools/register_webcam.yaml', help='filename of a YAML configuration for registration')
+    parser = argparse.ArgumentParser(
+        description="Register a webcam to the robot"
+    )
+    parser.add_argument(
+        "--config_filename",
+        type=str,
+        default="cfg/tools/register_webcam.yaml",
+        help="filename of a YAML configuration for registration",
+    )
     args = parser.parse_args()
     config_filename = args.config_filename
     config = YamlConfig(config_filename)
 
-    T_cb_world = RigidTransform.load(config['chessboard_tf'])
+    T_cb_world = RigidTransform.load(config["chessboard_tf"])
 
     # Get camera sensor object
-    for sensor_frame, sensor_data in config['sensors'].iteritems():
-        logging.info('Registering {}'.format(sensor_frame))
-        sensor_config = sensor_data['sensor_config']
-        reg_cfg = sensor_data['registration_config'].copy()
-        reg_cfg.update(config['chessboard_registration'])
+    for sensor_frame, sensor_data in config["sensors"].iteritems():
+        logging.info("Registering {}".format(sensor_frame))
+        sensor_config = sensor_data["sensor_config"]
+        reg_cfg = sensor_data["registration_config"].copy()
+        reg_cfg.update(config["chessboard_registration"])
 
         try:
             # Open sensor
-            sensor_type = sensor_config['type']
-            sensor_config['frame'] = sensor_frame
-            logging.info('Creating sensor')
+            sensor_type = sensor_config["type"]
+            sensor_config["frame"] = sensor_frame
+            logging.info("Creating sensor")
             sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
-            logging.info('Starting sensor')
+            logging.info("Starting sensor")
             sensor.start()
             intrinsics = sensor.color_intrinsics
-            logging.info('Sensor initialized')
+            logging.info("Sensor initialized")
 
             # Register sensor
-            resize_factor = reg_cfg['color_image_rescale_factor']
-            nx, ny = reg_cfg['corners_x'], reg_cfg['corners_y']
-            sx, sy = reg_cfg['size_x'], reg_cfg['size_y']
+            resize_factor = reg_cfg["color_image_rescale_factor"]
+            nx, ny = reg_cfg["corners_x"], reg_cfg["corners_y"]
+            sx, sy = reg_cfg["size_x"], reg_cfg["size_y"]
 
-            img, _, _= sensor.frames()
+            img, _, _ = sensor.frames()
             resized_color_im = img.resize(resize_factor)
             corner_px = resized_color_im.find_chessboard(sx=nx, sy=ny)
             if corner_px is None:
-                logging.error('No chessboard detected in sensor {}! Check camera exposure settings'.format(sensor_frame))
+                logging.error(
+                    "No chessboard detected in sensor {}! "
+                    "Check camera exposure settings".format(sensor_frame)
+                )
                 exit(1)
             webcam_corner_px = corner_px / resize_factor
 
             # Compute Camera Matrix for webcam
-            objp = np.zeros((nx*ny,3), np.float32)
+            objp = np.zeros((nx * ny, 3), np.float32)
             xstart = -sx * (nx / 2 - ((nx + 1) % 2) / 2.0)
             xend = sx * (nx / 2 - ((nx + 1) % 2) / 2.0 + 1)
             ystart = -sy * (ny / 2 - ((ny + 1) % 2) / 2.0)
             yend = sy * (ny / 2 - ((ny + 1) % 2) / 2.0 + 1)
             filler = np.mgrid[ystart:yend:sy, xstart:xend:sx]
-            filler = filler.reshape((filler.shape[0], filler.shape[1] * filler.shape[2])).T
-            objp[:,:2] = filler
+            filler = filler.reshape(
+                (filler.shape[0], filler.shape[1] * filler.shape[2])
+            ).T
+            objp[:, :2] = filler
 
-            ret, rvec, tvec = cv2.solvePnP(objp, webcam_corner_px, intrinsics.K, None)
+            ret, rvec, tvec = cv2.solvePnP(
+                objp, webcam_corner_px, intrinsics.K, None
+            )
             mat, _ = cv2.Rodrigues(rvec)
-            T_cb_cam = RigidTransform(mat, tvec, from_frame='cb', to_frame=sensor_frame)
+            T_cb_cam = RigidTransform(
+                mat, tvec, from_frame="cb", to_frame=sensor_frame
+            )
             T_cam_cb = T_cb_cam.inverse()
             T_camera_world = T_cb_world.dot(T_cam_cb)
 
-            logging.info('Final Result for sensor %s' %(sensor_frame))
-            logging.info('Translation: ')
+            logging.info("Final Result for sensor %s" % (sensor_frame))
+            logging.info("Translation: ")
             logging.info(T_camera_world.translation)
-            logging.info('Rotation: ')
+            logging.info("Rotation: ")
             logging.info(T_camera_world.rotation)
 
-        except Exception as e:
-            logging.error('Failed to register sensor {}'.format(sensor_frame))
+        except Exception:
+            logging.error("Failed to register sensor {}".format(sensor_frame))
             traceback.print_exc()
             continue
 
         # save tranformation arrays based on setup
-        output_dir = os.path.join(config['calib_dir'], sensor_frame)
+        output_dir = os.path.join(config["calib_dir"], sensor_frame)
         if not os.path.exists(output_dir):
             os.makedirs(output_dir)
-        pose_filename = os.path.join(output_dir, '%s_to_world.tf' %(sensor_frame))
+        pose_filename = os.path.join(
+            output_dir, "%s_to_world.tf" % (sensor_frame)
+        )
         T_camera_world.save(pose_filename)
-        intr_filename = os.path.join(output_dir, '%s.intr' %(sensor_frame))
+        intr_filename = os.path.join(output_dir, "%s.intr" % (sensor_frame))
         intrinsics.save(intr_filename)
 
         sensor.stop()
diff --git a/tools/test_ensenso.py b/tools/test_ensenso.py
index 6d69e68..c2c1d77 100644
--- a/tools/test_ensenso.py
+++ b/tools/test_ensenso.py
@@ -7,23 +7,22 @@
 import sys
 import time
 
-from perception import EnsensoSensor, PhoXiSensor
+from perception import RgbdSensorFactory
 from visualization import Visualizer2D as vis2d
 from visualization import Visualizer3D as vis3d
 
+
 def main(args):
     # set logging
     logging.getLogger().setLevel(logging.INFO)
-    rospy.init_node('ensenso_reader', anonymous=True)
+    rospy.init_node("ensenso_reader", anonymous=True)
 
     num_frames = 10
-    #sensor = PhoXiSensor(frame='phoxi',
-    #                     size='small')
-    sensor = EnsensoSensor(frame='ensenso')
+    sensor = RgbdSensorFactory("ensenso", cfg={"frame": "ensenso"})
     sensor.start()
 
     total_time = 0
-    for i in range(num_frames):        
+    for i in range(num_frames):
         if i > 0:
             start_time = time.time()
 
@@ -31,23 +30,24 @@ def main(args):
 
         if i > 0:
             total_time += time.time() - start_time
-            print('Frame %d' %(i))
-            print('Avg FPS: %.5f' %(float(i) / total_time))
-        
+            print("Frame %d" % (i))
+            print("Avg FPS: %.5f" % (float(i) / total_time))
+
     depth_im = sensor.median_depth_img(num_img=5)
-    point_cloud = sensor.ir_intrinsics.deproject(depth_im) 
+    point_cloud = sensor.ir_intrinsics.deproject(depth_im)
     point_cloud.remove_zero_points()
 
     sensor.stop()
 
     vis2d.figure()
     vis2d.imshow(depth_im)
-    vis2d.title('PhoXi - Raw')
+    vis2d.title("Ensenso - Raw")
     vis2d.show()
 
     vis3d.figure()
     vis3d.points(point_cloud, random=True, subsample=10, scale=0.0025)
     vis3d.show()
-    
-if __name__ == '__main__':
+
+
+if __name__ == "__main__":
     main(sys.argv)
diff --git a/tools/test_realsense.py b/tools/test_realsense.py
index 83f0b47..25f24df 100644
--- a/tools/test_realsense.py
+++ b/tools/test_realsense.py
@@ -19,11 +19,11 @@ def main():
     assert ids, "[!] No camera detected."
 
     cfg = {}
-    cfg['cam_id'] = ids[0]
-    cfg['filter_depth'] = True
-    cfg['frame'] = 'realsense_overhead'
+    cfg["cam_id"] = ids[0]
+    cfg["filter_depth"] = True
+    cfg["frame"] = "realsense_overhead"
 
-    sensor = RgbdSensorFactory.sensor('realsense', cfg)
+    sensor = RgbdSensorFactory.sensor("realsense", cfg)
     sensor.start()
     camera_intr = sensor.color_intrinsics
     color_im, depth_im, _ = sensor.frames()
@@ -31,12 +31,12 @@ def main():
 
     print("intrinsics matrix: {}".format(camera_intr.K))
 
-    fig, axes = plt.subplots(1, 2)
+    _, axes = plt.subplots(1, 2)
     for ax, im in zip(axes, [color_im.data, depth_im.data]):
         ax.imshow(im)
-        ax.axis('off')
+        ax.axis("off")
     plt.show()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()
diff --git a/tools/test_weight_sensor.py b/tools/test_weight_sensor.py
index 9cacf05..734c80b 100644
--- a/tools/test_weight_sensor.py
+++ b/tools/test_weight_sensor.py
@@ -4,20 +4,19 @@
 """
 import logging
 import rospy
-import sys
 import time
 
 from perception import WeightSensor
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     # set logging
     logging.getLogger().setLevel(logging.INFO)
-    rospy.init_node('weight_sensor', anonymous=True)
+    rospy.init_node("weight_sensor", anonymous=True)
 
     # sensor
     weight_sensor = WeightSensor()
-    weight_sensor.start()    
+    weight_sensor.start()
     for i in range(10000):
-        print 'Total weight:', weight_sensor.total_weight()
+        print("Total weight:", weight_sensor.total_weight())
         time.sleep(0.1)
     weight_sensor.stop()